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SYNOPSIS 


The dual-arm manipulators have an important role to play 
in the modern industrial applications. A single-arm manipulator 
has limitations in carrying a heavy pay-load or holding objects 
with odd geometrical shapes. Besides offering easier handling in 
such cases, the dual-arm manipulators also provide the necessary 
flexibility in assembly operations where the assembly task is 
performed by both the arms and hence the redundant degrees of 
freedom are available. 

This thesis attempts to solve two problems of dual-arm 
manipulators. In the first problem, a kinematically redundant 
dual-arm manipulator is considered and the kinematic inverse 
solutions are investigated. The second problem is that of 
trajectory planning where the dual-arm system has torque 
redundancy and the results are sought for minimum time and minimum 
electrical energy criteria. 

The first problem considers a peg-in-hole type assembly 
operation where the relative movement between the end-effectors 
of two arms is specified. Due to kinematic redundancy the inverse 
solution is not unique and hence the assembly can take place 
anywhere in the workspace. The kinematic inverse problem in this 
work is posed as an optimization problem. The relative path is 
specified by a sequence of knot points. Three optimization 
criteria have been used to do a comparative study. The three 
optimization procedures respectively lead to minimum velocity norm 
solution, minimum acceleration norm solution, and increased 



‘ i V 


man i pu lab i 1 i ty solution. All these three schemes optimize locally 
on the path. The numerical results for inverse kinematics are 
obtained for a planar case where two arms, each with three 
revolute joints, are employed for a peg-in-hole assembly in a 
plane. The minimum acceleration norm solution is shown to result 
in low fluctuations in the joint velocities at the expense of 
somewhat larger magnitudes of velocities as compared to minimum 
velocity norm solutions. Maximizing man i pu lab i 1 i ty leads to large 
joint movements needed to reconfigure the arms. Instead, 
increasing the man i pu lab i 1 i ty gradually, as the assembly 
progresses, is shown to be a better strategy. 

The major part of this thesis is devoted to the 
trajectory planning problem of a dual-arm manipulator where each 
arm has six degrees of freedom. The task assigned is that of 
carrying a rigidly grasped object along a specified path. The two 
arms alongwith the rigidly held object between end effectors form 
a closed chain. This closed chain does not have kinematic 
redundancy. But as ail the twelve joints of the dual-arm system 
are considered to be active joints, there is torque redundancy in 
the system, i.e. the resultant force exerted on the object (which 
has six components) is exerted by the twelve joint torques and so 
the joint torque distribution is not unique. The trajectory 
planning problem (i.e., determination of time histories of joint 
torques, velocities etc. along the specified path) is therefore 
posed with some additional criteria, as an optimization problem. 
The formulation of the problem is done in the following two 


stages . 
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In the first stage, the torque redundancy is resolved 
according to either the criterion of minimum norm of joint torque 
vector or that of minimum actuator power. At this stage, assuming 
that the object trajectory (position, velocity and acceleration) 
is known, the complete nonlinear dynamics of the closed chain (two 
arms and the object) is written and a closed form expression is 
obtained for the joint torques resolved according to above two 
criteria. 

The second stage involves determination of the time 
histories of joint variables. To this end. the trajectory 
planning problem is posed as minimum time problem, or minimum 
energy (electrical energy) problem or minimum time and energy 
problem. This optimization problem is subject to both equality 
and inequality constraints. The equality constraints are the 
torque redundancy resolution relations and the condition that the 
joint velocities be zero at the beginning and end of the 
trajectory. The inequality constraints are due to the limits on 
the joint torques and joint velocities. 

The continuous dynamic model is then discretized to 
implement numerical schemes. For this, the specified object path 
is divided into number of knot points. The inverse of time 
intervals between two successive knot points, i.e. the reciprocal 
time intervals are taken as a set of independent variables. All 
the relevant equations of optimization formulation are now written 
in discrete form in terms of the reciprocal time intervals with 
the help of implicit trapezoidal (smoothing) rule of 
discretization. This discretization yields the velocities as 
linear functions of the set of reciprocal time intervals, whereas 



the objective function and the torque relations are obtained as 
nonlinear functions. 

The optimization scheme adopted is that of successive 
linear programming. For this purpose the nonlinear equations are 
linearized with respect to the set of reciprocal time intervals. 
An initial guess on reciprocal time intervals is made and then 
linear programming is applied to improve the solution iteratively 
till convergence is achieved. The convergence process is also 
studied through "warm start” where the initial guess values are 
guided by the trial runs. 

The numerical results are presented for a dual-arm 
system where each arm is modelled as PUMA-560 manipulator. The 
rigid object has the shape of rectangular parallelepiped. The 
object is grasped rigidly between the two end effectors. In the 
first example, a vertically straight trajectory of the object, 
with fixed orientation along the path, is considered. With the 
optimization scheme outlined above, three solutions corresponding 
to minimum time problem, minimum energy problem and minimum time 
and energy problem are presented. This example is discussed in 
detail for the effect of changing the number of knot points on the 
object path and the viability of warm starting the iterative 
procedure by making an improved guess at the starting stage. The 
successive linear programming approach is shown to be particularly 
suitable for minimum time problems where the convergence can be 
made very fast. The minimum energy term slows down the 
convergence substantially. The travel time in the minimum time 
case is obviously restricted by the limits on the joints torques 
and joint velocities. In minimum energy case, the tendency of the 
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dual-arm system to move very slow, in order to consume low energy, 
is opposed by the gravity induced torques which tend to speed up 
the system. The time of travel is much longer (about two and half 
times) for minimum energy case as compared to that for minimum 
time case. The minimum time and energy case yields an 
intermediate set of results. 

The numerical results are presented for two more 
examples. The second example is an extension of the first example 
where the object orientation is also changed along the vertical 
path. The resulting effect is in changing the torque and velocity 
distributions and a different set of joints now reach the velocity 
limits without any joint reaching the torque limit. The third and 
the last example considers a parabolic path for the object in a 
curvilinear trajectory. This trajectory has been selected to 
study a case where the velocities undergo reversal of sign. 
Finally some conclusions and scope of further work are presented. 
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CHAPTER 1 


tt'ITRODUCTION 


1.1. Int.roduct.ion 

The robotic systems have been employed in industrial 
applications for a long time. There are now variety of types of 
robotic systems with their applications being spread to much wider 
areas. One important class is that of multi-arm robotic systems 
which encompasses cooperating manipulators, mechanical hands and 
walking robots. 

The design and development of robotic systems has been 
greatly influenced by the structure and working behaviour of 
humans and other living organisms. For example, the walking robot 
design is guided by the walking traits of quadrupeds, hexapods 
etc. Likewise the dexterous mechanical hands are along the lines 
of human fingers. Thus it is natural that like the two human hands 
there should be two-arm <or even multi-arm) cooperating 
manipulators. The main feature common to all these robotic systems 
is that of coordinated motion but the discussions in this thesis 
are restricted to cooperating manipulators with two arms, which 
will be refereed to as dual-arm systems. 

The coordinated nration of dual-arm systems has been 
broadly classified into two categories as tightly coordinated 
motion and loosely coordinated motion. The tightly coordinated 
motion corresponds to cases where the two arms perform a common 
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operation, e.g. two hands handling a common object or two arms 
together performing an assembly operation. The loosely coordinated 
motion implies that the two arms share a common workspace but 
perform independent tasks like while one arm loads the other arm 
unloads . 


The 

t i ght ly 

coord i nated 

dual-arm manipulators 

are 

part i cu lar 1 y 

needed when 

the object 

being handled is 

e i ther 

too 

heavy or too 

long for a 

single arm 

manipulator. The 

open 

cha i n 

structure of 

a single arm itself nnakes it unsuitable 

for 

high 


payload duty operations. The objects which have large dimension(s) 
are difficult to hold in a balanced way with a single arm and 
again the dual-arm manipulator is useful. Besides this, many 
assembly operations also will be easier with dual-arm manipulators 
so that assembly is carried out by the two arms configured in the 
most appropriate positions in the workspace (for example, the 
configurations which are away from singularities). Moreover. the 
two arms can also be used in independent operations in a loosely 
coordinated motion thereby having a larger workspace and faster 
operat i ons . 

The arms can be made similar if identical features are 
required in both arms like load sharing, control etc. Alternately, 
the two arms can be made complementary to each other like human 
arms. For example, one arm may have higher payload capacity but 
slower motion and lower precision while the other arm is light, 
fast and precise such that the first arm takes care of a heavier 
part and the other arm carries out the compensatory precise motion 
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required for assembly. 

In many applications, a single arm manipulator is 
provided redundancy in order to have more maneuverability to avoid 
singularities, and to have different choices in task distribution. 
The multi degrees-of-f reedom system is essentially broken into 
subsystems of smaller degrees of freedom to result in multi-arm 
manipulators. The basic problem of these multi-arm systems is in 
producing coordinated motion of the subsystems. This coordination 
is needed both at the kinematic level and at the force level. 

If the number of task variables is less than the total 
degrees of freedom of the dual-arm system then there is kinematic 
redundancy. The kinematic inverse solution is then not unique and 
some additional criteria of distributing the task have to be 
imposed to solve the problem. This offers various choices like 
having the solution that produces minimum norm of joint 
velocities, or yields best manipulable arm configurations etc. In 
addition, a paramount problem is collision avoidance. Kinematic 
redundancy again can be used to configure the arms in a collision 
free state. 

The dual-arm manipulators usually have torque redundancy 
(while it may not have kinematic redundancy, for example two six 
degrees-of-f reedom arms forming a closed chain through a common 
rigidly held object), i.e. the number of actuated joints is more 
than what is required to produce the motion. Therefore, the 
coordinated motion requires proper force distribution among the 
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actuated joints and/or the held object. The inverse dynamics 
problem of determining the actuator f orces/torques is generally 
posed as a constrained force optimization problem. The force 
constraint equations are in the form of equality constraints as 
dynamic equations of the systems, inequality constraints on the 
torque limits of actuators, and inequality constraints arising due 
to interactions with the held object which is not rigidly grasped 
as in case of mul t i -f i ngered hands. 

There are two approaches to the trajectory planning of 
dual-arm manipulators. In the first approach the path planning is 
done without considering dynamics and then a path tracker (based 
on PID scheme, computed torque scheme etc.) is employed to control 
the systems. The second approach considers the manipulator 
dynamics and trajectory planning problem is reposed as an 
optimization problem to take care of redundancy in actuation. The 
optimum trajectory is usually based upon the minimum time and 
minimum energy criteria, the path may or may not be specified and 
trajectory planning yields the time histories of joint variables 
like position, velocity and torque. 

This work attempts to address two problems of dual-arm 
manipulators. The first one is that of resolving the kinematic 
redundancy and the second problem is of trajectory planning with a 
dynamic model of the system. 

1»2 Literature Survey 

This review of literature is focused mainly upon the 
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topics related to redundant nnanipulators and two arm manipulators. 
The topics are discussed below under three major headings, namely 
i) kinematics, ii) dynamics and iii) trajectory planning. 

Kin&mxtics 

The kinematic studies can be broadly classified into two 
categories as i) kinematics of nonredundant arms, and ii> 
kinematics of redundant arms. A dual-arm manipulator may be 
kinematically nonredundant if it forms a closed chain when an 
object is rigidly held between two arms, and the closed chain thus 
formed has six degrees of freedom in the general case of space 
motion of the object. This study will address only the works 
related to kinematically redundant arms. 

Lee C19891 presented a comprehensive survey on inverse 
kinematic solution of redundant arms. The solutions of the 
under-defined set of kinematic equations have traditionally been 
obtained by imposing additional constraints based upon some 
performance criteria. He classified these approaches into four 
major categories as follows; 1) The Jacobian pseudo- i nverse 
solution as the minimum norm solution C Yosh i kawa , 1 984] . Z> The 
modified Jacobian pseudo- i nverse solution in which the Jacobian 
null space solution is added to the Jacobian pseudo- i nverse 
solution to maximize a performance index indicating 
man i pu labi 1 i ty , collision, joint stop avoidance, etc. CWhitney. 
1972]. 5) The extended Jacobian solution in which extra 
constraints obtained from the condition of optimal joint 
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configurations are addled to the forward kinematics to yield the 
extended Jacobian CBailiieul, 198^]; in case only one degree of 
freedom of redundancy exists, the extended Jacobian provides an 
explicit solution. 4> The optimization of the lagrangian formed by 
a certain performance index and by forward kinematics as 
constraints CChang , 1 986 ] . Kazerounian and Wang [1988] considered 
global optimization of joint velocities and concluded that 
globally optimum solutions are described as the minimum 
acceleration norm solution. Chevallerean and Khalil [1988] 
extended the pseudo- i nverse kinematic procedures for redundant 
arms to take care of several criteria, including that of 
increasing the man i pu labi 1 i ty . 

Lee [1989] and Chiacchio et al. [1991a] extended the 
concept of manipulabi 1 i ty ellipsoids for single arm to the case of 
dual-arm and multi-arm manipulators, respectively. Lee [1989] 
defined task-oriented dual-arm manipulabi 1 i ty as the measure of 
geometrical similarity between the desired manipulabi 1 i ty 
ellipsoid and the dual-arm man i pulabi 1 i ty ellipsoid and this 
task-oriented dual-arm man i pu labi 1 i ty measure approximately 
represented the volume of intersection between two individual 
manipulabi 1 ity ellipsoids. This was used then to optimize the 
dual-arm joint configurations. Chiacchio et al. [1991a] adopted a 
global kinetostatic formulation of the closed chain created by 
multiple tightly cooperating arms to define external and internal 
force manipulabi 1 i ty ellipsoids. The corresponding absolute and 
relative velocity man ipulabi 1 i ty ellipsoids were derived on the 



■ 7 


basis of duality principle. These were utilized to determine 
optimal postures for redundant multi-arm system. 

Ahmad and Luo C19891 dealt with the problem of 
coordinating a positioning table and a seven axis manipulator. The 
cartesian motions of the manipulator were selected from a 
cartesian coordinate nonlinear optimization process to maximize 
the manipulator maneuverability and avoid manipulator 
singularities and track motion limits. This process was claimed to 
be advantageous over pseudo- i nverse techniques from the point of 
view of inaccuracies and computational loads, Suh and Shin C1989] 
considered the case of coordinated motion of dual-arm carrying a 
solid object held at two ends. When the object was held rigidly 
there was no kinematic redundancy. In this work while one arm 
(leader) was moving, the second arm (follower) was changing grasp 
position along the object surface while supporting and thereby 
added one more degree of freedom to the system and consequently 
gave more maneuvering capability. 

1 * 2 * 2 * Dynamics 

The dynamics of manipulators is generally discussed in 
two groups. In the first, the dynamic models and the associated 
forward and inverse dynamics issues are studied. The second group 
addresses the control aspects of dynamic models of manipulators. 
Since the present work focuses only on the inverse dynamics of 
dual-arm manipulator, the relevant literature is reviewed first, 
and a brief review of control aspects then follows. 
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A closed chain mechanism may not be kinematically 
redundant e.g., a dual-arm manipulator rigidly holding an object 
in space, where each arm has six degrees of freedom. But if all 
the manipulator joints are active then torque redundancy does 
exist. The following discussions are limited to the works related 
to the dynamics of manipulators having redundancy in actuation. 

Hayati C1986] concentrated on the dynamic model of a 
closed chain (dual-arm with rigidly held object between arms) 
through Lagrange formulation, while taking care of the interaction 
forces between the object and the end effector. The task space was 
decomposed into a position control subspace and a force control 
subspace; each subspace being controlled independently. Freeman 
C19871 obtained the dynamic model through principle of virtual 
work and an inner product operator. Koivo and Unseren [1990] 
studied the coupling effects between the manipulators due to 
common rigid load and their impact on the modelling of the closed 
chain. Huang C1992] presented a unified formulation for 
constrained robot systems, including dual-arm manipulator holding 
an object. Thus all the possible systems were of closed chain 
type. The equations of motion and constraint equation (of 
algebraic type) were combined to form a "singular system of 
differential equations” and shown to be an appropriate model to 
describe the system. 

Pennock and Ryuh [1987] considered only six joints as 
active joints (where torques are to be applied ) and rest of the 


9 


joints were taken as passive joints. Luh and Zheng [1987] used 
holonomic constraints to establish the coordinated relationship 
between the two arms, one called as leader and other as follower. 
Thus for a desired trajectory they obtained the complete kinematic 
relations including accelerations and then used Lagrange 
formulation to get the joint torques of each arm. Zheng and Luh 
[1988] used a similar approach to consider the dynamics of two 
arms involved in assembly. Unseren and Koivo [1989.1991] proposed 
a concept of pseudo velocity and pseudo acceleration to build the 
reduced order model for a closed chain dynamics. Then a decoupled 
control was discussed based upon the reduced order model and the 
generalized contact forces. Walker et al . [1991] concentrated an 
internal loading of the object in a multi-arm configuration. They 
introduced "nonsqueezing" pseudo- i nverse consistent with the 
kinematic constraints, and developed motion and squeezing 
effects. Chiacchio et al. [1991b] developed a systematic method to 
perform dynamic analysis in the task space for a system composed 
of multiple arms holding a rigid object. They introduced the term 
dynamic man i pu labi 1 i ty ellipsoid to obtain quantitative indices of 
the system capability, in each configuration, of performing object 
accelerations along given task space directions. The ellipsoid was 
derived on the basis of the mapping of object accelerations on to 
joint driving torques via the proper k i heto-stat i c and dynamic 
equations of the system. Li et al . [1991] performed the dynamic 
workspace analysis of multi-arm robots (closed chain model) to 
find the maximum force that the robots can jointly generate at 


10 


each point within the common workspace. The problem was posed as 
an optimization problem with constraints on torque limits of 
joints and the maximum internal force that the object (in contact 
with arms) can withstand. 

A common approach to resolve the torque redundancy has 
been generally through posing an optimization problem. Orin and Oh 
[19813 analysed the dynamics of legged vehicles, or equivalently 
multiple arm manipulator system forming a closed kinematic chain 
with a rigid body object. The input torques were determined by 
taking care of the redundancy in torques through an optimization 
problem which minimizes the power consumption and optimizes the 
load sharing between several chains. The works along these lines 
differ mainly in selecting the optimal performance criteria and in 
the method of solution. Nakamura et al . C19873 minimized internal 
forces, later Nakamura [19883 minimized the strain energy of the 
object; many researchers used the power or electrical energy as 
the minimization criteria as by Orin and Oh [19813, Carignan and 
Akin [1988, 19893, Zheng and Luh[19893 etc. An optimal torque 
distribution by using a quadratic cost function of torques was 
obtained by Zheng [19893, Carignan and Akin [1988, 19893 and 
Djurovic et al . [19903. Zheng and Luh [19883 proposed to reduce 
the computational complexity by minimizing the forces on the 
object at the expense of energy consumption. Carignan and Akin 
[19883 had also added the force minimization and thus broken the 
problem into two suboptimal problems. Nahon and Angeles [1992a3, 
on the other hand, proposed that minimization of power losses in 
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The optimization algorithms used in the literature 
include linear programming COrin and Oh, 19813, nonlinear 
programming CNakamura et al.l987], compact dual linear programming 
CCheng and Orin, 19893. quadratic programming CNahon and Angeles, 
199253 and linear optimal control theory CTarn et.al 19873. The 
debate as to which method is better is inconclusive. Cheng and 
Orin C19893 proposed that Compact-Oual LP method is 
computationally efficient while optimizing the contact forces 
between arms and the object. It was pointed out by Nahon and 
Angles C1992b3 that quadratic programming is better for handling 
the situations where minimization of internal forces also appears. 
Instead of an end effector, which can grasp rigidly, they 
considered fingers to hold an object and this had inequality 
constraints appearing on the constraint forces. 

Tarn et al . C19873 modelled the two-arm closed chain 
such that the contact of the end effector with the object was 
assumed to be a revolute joint. The dynamic equations were 
linearalized and decoupled using differential geometric system 
theory. Then linear optimal control theory was used to determine 
control law for torques. 

Various control schemes for multi-arm manipulators have 
been proposed by researchers. Even-though the present work does 
not deal with this aspect, a brief summary of these works is in 
order for completeness. Ishida C19773 studied the force control of 
coordinating arms where instead of using position control the 
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motor torques at each joint were evaluated. Uchiyama and Dauchez 
C1988] proposed hybrid position/force controller for coordination 
of two arms and later Uchiyama and Yamashita C19913 presented 
experimental results of hybrid control for robust holding of a 
common object by two arms; these schemes did not consider the 
dynamic problem. Laroussi et al . E1988] studied the dynamic model 
of a planar two arm robot and used linear state feedback for 
control of the system. Seraji C1988]. Hu and Goldenberg C1989], 
and Walker et al . C1989] discussed adaptive control schemes for 
coordinated motion and force control, Tao and Luh C19911 and Koga 
et al . C1992] discussed mode 1 -ref erence adaptive controllers for 
similar considerations. Kazerooni C19883 and Tao et al . C1990] 
presented compliance control of two arms carrying an object where 
the force control of one arm at the contact point was of main 
interest. Schneider and Cannon C19923 discussed the implementation 
of impedance control. Nonlinear feedback techniques which 
linearize and decouple the nonlinear dynamic equations were 
studied by Yun C1989] and Yun and Kumar C1991]. 

The dynamics and control of multi-arm manipulators have 
many similar features with those of mu 1 1 i f i ngered hands, e.g. Cole 
et al . C1992], Montana C19923 and with walking machines, e.g. Shih 
and Gruver C19923, Kajita et al . C19923 etc. 

1 * 2* 3 Traj&c tory plomning 

The works in trajectory planning area can be broadly 
classified into two main categories. The first category addresses 
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the problem of finding a safe path of the manipulatorCs) in the 
work space and is called obstacle avoidance problem. In the second 
category, trajectory planning is concerned with finding the time 
histories of joint variables on a safe path which either may have 
been specified or is to be planned in an obstacle free 
environment. These time histories (joint position, joint torque 
etc.) are typically determined through some criteria like minimum 
time criterion. In this subsection, first a brief review of 
obstacle avoidance problems is presented and then the works 
related to finding time histories of joint variables are reviewed. 

The classical problem of collision avoidance, both in 
navigation and manipulation, has been a topic of interest to many 
researchers. The methods based on characterizing free space have 
been dealt by Schwartz and Sharir 111985a, 1985b ,1985c], Udapa 
[1977] ,Lozano-Perez [1981,1985], Lazano-Perez and Wesley [1979], 
Brooks [1985,1985], Wenger and Chedma i 1 [ 1 991 ] and Faverjon [1984]. 
Khatib [1986] proposed a method of path planning using a potential 
field concept which was extended by Volpe and Khosla [1990], Hwang 
and Ahuja [199Z], Barraquand et al . [199Z], Kim and Khosla [199Z]. 
Gilbert and Jhonson [1985] presented a trajectory planning scheme 
minimizing a cost function in the presence of obstacles. Besides 
this, path planning with incomplete information about the 
environment has also been dealt by Lumelsky, for example in 
Cheung and Lumelsky [1990]. 

In the area of collision free motion planning for a 
multi-robot system, Freund and Hoyer [1985, 1986, 1988] formulated 
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the problem as a "find path" problem. Erdmann and Lozano— Perez 
C1987] explored the motion planning problem for multiple moving 
objects by using configuration space-time technique to handle 
timevarying constraints imposed on a moving object. Fortune et al . 
C1986!] described an algorithm for finding collision free 
trajectories for two planar arms by characterizing the 
combinatorial structure of the configuration space of the two 
arms. Tournassoud C1986] described a technique for coordinating 
multiple moving object including manipulators by defining 
separating planes at each moment and ensuring that the object 
stays on the opposite sides of them. O'Donnell and Lozano-Perez 
C1989] posed the trajectory planning problem as scheduling problem 
in which space was the shared resource and the method decoupled 
the path specification step from the trajectory specification 
step. The collisions were avoided by using time without changing 
the path. Chien and Xue C1992] investigated the problem of 
planning a collision free path for a dual planar arm carrying a 
small object. The arms and the object formed a five link closed 
chain which had to move in a space full of unknown obstacles. 
Shin and Bien C19893 planned collision free trajectory of two arms 
along designated path. They introduced the concept of virtual 
obstacle to consider possible collision between manipulators and 
adopted virtual coordination space to select a collision free 
coordination curve. Along this curve they planned the minimum time 
trajectory satisfying all the dynamic constraints. Lee and Lee 
C1987] and Lie C1986] modeled the robots as spheres and restricted 
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each robot to move along a straight path. They approximated the 
collision regions with rectangles and showed that delaying one 
robot at its initial position can achieve a smaller travel time 
than other collision avoidance strategies. Shin and Zheng [19923 
have extended the idea of Lee and Lee C19873 with accurate 
geometric models of robot and allowing arbitrary paths and showed 
that delaying one robot at its initial position can achieve 
minimum travel time among ail collision avoidance strategies if 
collision region is connected. Bien and Lee [19923 solved the 
problem on the similar line. They started by planning time optimal 
trajectory of each robot independently satisfying the dynamic 
constraints and then transformed the collision region (defined in 
the coordination space) into t ime-versus-trave 1 ed length space 
with the preplanned velocity profile of one robot. In this 
transformed collision region, a minimal delay in the starting time 
of the other robot is determined to avoid collision. Jouaneh et 
al . [19903 considered the problem trajectory planing between a 
robot and a positioning table. They have posed the trajectory 
planing problem as minimum time and minimum energy trajectory 
planing problem and solved by one variable dynamic programing 
approach. They studied this on a three degrees of freedom robot 
and a two axes linear positioning table. 

Trajectory planning problems in an obstacle free space 
with dynamic model of a single arm manipulator were studied by 
several researchers. Kahn and Roth [19713 studied minimum time 
trajectory planning problem for manipulator travelling between two 
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end points in which the path was not specified. The constraints 
imposed were the maximum forces/torques available. Luh and Lin 
C19811 and Luh and Walker C19773 considered cartesian velocities 
and accelerations as constraints instead of forces/torques which 
required experimental identification of cartesian velocity and 
acceleration bounds. Kim and Shin C19891 developed the method for 
minimum time path planning in joint space. The path was given in 
joint space by corner points. They specified absolute tolerance in 
the path deviation at each corner point and derived joint 
acceleration bounds from the arm dynamics to utilize the robot 
capabilities. Vukobratovic and Kircanski C1982] applied dynamic 
programming for solving optimal trajectory planning problem over a 
specified path, the forces and torques were bounded and the travel 
time was also given. Bobrow et al.C1989] devised a specific 
technique to solve minimum-time trajectory planning problem for a 
manipulator following a prescribed path and took the bounds on the 
torque function of the joint angle and angular velocities. Shin 
and McKay [1986] applied dynamic programming to optimal trajectory 
generation. They represented the path by a unique scalar 
parameter and rewrote the dynamic equations in terms of this 
parameter and its time derivative. The resulting constraints and 
costs were also represented by these two parameters, and the 
dynamic programming method applied to grid spanned by them. The 
technique was limited to path which could be parameterized. Singh 
and Leu [1987] also solved optimum path planning problem by 
dynamic programming but instead of parameterized path they 
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approximated the given path by discrete points in the joint 
space. Pfeiffer and Johanni [1987] solved the minimum time problem 
with constant torque limits by constructing switching curves. 
Hollerbach [1984] identified a time scaling property for the 
dynamics of a manipulator executing a planned trajectory. The 
constant time scaling was utilized to ensure dynamically feasible 
manipulator speeds along a prescribed tra j ectory . Tan and Potts 
[1988] proposed a minimum time trajectory planner by discrete 
approach in contrast to other models which use continuous time 
model. It included joint velocity, joint torque, and joint jerk 
constraints and posed the minimum time trajectory planning problem 
as a nonlinear optimization problem, solved by the "method of 
approximate programming". Tan and Potts [1989] later modified 
their trajectory planner by planning the position of the knots 
such that the time intervals between two consecutive knot points 
were same. The trajectory planning problem thus was a single 
nonlinear programming problem applied to a six degrees of freedom 
manipulator for minimum time and minimum energy case. The 
trajectory tracking problem for a single arm manipulator was dealt 
by Johansson [1990] as a quadratic optimal control problem and 
Chen et al . [1992] also considered the time-optimal control law 

for tracking problems. Koivo and Arnautovic [1991] discussed the 
dynamic optimal control for redundant manipulators where they 
minimized the tracking error along the path together with maximum 
energy. Gupta and Luh [1991] also used optimal control theory for 
closed loop control of redundant manipulators where the redundancy 
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was resolved at the joint acceleration level and then the tracking 
error and the control was minimized along the path. 

Not much work has been reported in the trajectory 
planning problems of dual-arm manipulators forming a closed chain 
through a grasped object, Lim and Chyung C1988] dealt with the 
problem of trajectory planning of a dual-arm manipulator carrying 
an object as a nonlinear optimization problem but studied only the 
kinematic aspects. Benhabib and Tabarah C19893 also studied the 
trajectory planning for point-to point motion of dual-arm 
manipulators considering only the kinematic aspects in the 
trajectory planning., They used cubic polynomials for all joint 
trajectories as means of obtaining continuous joint velocities and 
accelerations throughout the motion. The main objective of the 
optimization was to minimize the number of intermediate joint 
configurations with the primary constaint on joint trajectories 
such that the task space errors were within specified tolerance 
values. Moon and Ahmad C1991] considered a similar problem but had 
considered the constraints on joint torques. The method used was 
time scaling of the trajectory. They assumed that the velocity 
profile was given without considering dynamics and checked if the 
trajectory violated the joint torque limits. In case of torque 
limit violation they modified the trajectory by time scaling. This 
approach did not allow optimization of the trajectory according to 
minimum time or minimum energy criteria. 


1.3 Obj©ct-iv© and scop© of th© pr©s©nt work 



This work attempts to solve two problems of dual-arm 
manipulators. The first problem considers an assembly operation by 
two arms where the arms have kinamatic redundancy and in this 
problem only the kinamatic inverse solution is aimed at. The 
assembly operation is considered to be that of peg-in-hole 
insertion type. The relative movement between the end effectors of 
two arms is to be specified for the assembly operation. The aim is 
to show the advantage of assembly with kinematically redundant 
dual-arm robot where the same assembly operation is performed at 
different locations in the workspace depending upon the 
requirements. Traditionally .the solution is sought by posing an 
optimization problem. The present work considers three 
optimization criteria to resolve the kinamatic redundancy. These 
are used to obtain the minimum velocity norm solution .minimum 
acceleration norm solution and increased man i pu lab i 1 i ty solution. 
Maximization of manipulabi 1 i ty criterion leads to a nonlinear 
optimization (e.g. Lee C1989]) and. therefore, in the present work 
only the increase in man i pu lab i 1 i ty is sought. Though this process 
does not yield the maximum man i pu lab i 1 ty at intermediate 
configurations, but is expected to ensure better manipulable 
postures <away from the singularity configuration) through a much 
simpler quadratic optimization problem while keeping the joint 
movements small. 

The second problem attempts to plan the trajectory of a 
dual-arm manipulator where the two arms form a closed chain 
through a rigidly grasped object and the path, through which the 
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object is to be carried, is specified. Each arm has six degrees of 
freedom and thus the closed chain has no kinematic redundancy but 
does have torque redundancy as all the manipulator joints are 
considered to be active. Therefore, this trajectory planning 
problem involves two main aspects, <i> that of resolving the 
torque redundancy, and <ii) that of finding optimum time histories 
of joint variables (position, torque etc.). Though many 
researchers have investigated the torque redundancy in multi-arm 
systems (see Section l.Z.Z), the trajectory planning through 
optimum time histories of joint variables has been mainly confined 
to single arm manipulators (see Section I.Z.?) where if redundancy 
exists then the redundancy is in actuation as well as in 
kinematics and so the redundancy can be resolved in the kinematic 
level (e.g. acceleration). Thereafter the optimal control 
formulation can be used to solve the minimum time problem. Since 
the dual-arm system considered in this part of the present work 
does not have kinematic redundancy , the attempt is to first 
resolve the torque redundancy and then use this to formulate the 
optimization problems to solve minimum time and minimum energy 
problems. A discrete formulation is used to solve the optimization 
problem which includes all the joint constraints. 

1.4 Overview of thesis 

This thesis is divided into five chapters. The first 
problem, addressed to the inverse kinematics of redundant dual-arm 
manipulator in assembly operation is dealt in Chapter Z. The 
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formulation of kinematic equations of redundant dual-arm, method 
of solution and results are discussed in this Chapter. Chapter 5 
contains the formulation part of the second problem addressed to 
the trajectory planning problem of a dual-arm grasping a rigidly 
held object. This formulation mainly contains the derivation of 
dynamic equations of dual-arm and resolution of torque redundancy. 
The discrete models, for solving the problem numerically, are also 
developed in Chapter 3. Two PUMA-560 arms are used for simulation 
of the trajectory planning problem and the computed results are 
presented and discussed in Chapter 4. Finally Chapter 5 reports 
the concluding remarks and the scope for future work. 



CHAPTER 2 


INVERSE KINEMATICS OF REDUNDANT DUAL-ARM MANIPULATOR 


2»1 Introduction 

This chapter discusses the inverse kinematic solutions 
for a redundant dual -arm manipulator. Redundancy of the dual -arm 
means that the number of task coordinates is less than the number 
of joints of the dual-arm and hence the inverse solution is not 
unique. The problem is stated in Sec- 2.2. The position kinematics 
and the velocity relationships are discussed in Sec. 2.3 and Sec. 
2.4, respectively. The inverse kinematic solution is obtained in 
Sec. 2.5 by posing the inverse problem as an optimization problem, 
with three different objective functions corresponding to minimum 
velocity norm condition , minimum acceleration norm condition and 
increased man i pu lab i 1 i ty condition. The solution process outlined 
in Sec. 2.5 considers a general dual-arm manipulator. The results 
are presented in Sec. 2.6 for a planar dual-arm system and the 
inverse kinematic solutions obtained through different objective 
functions are compared. 

2. 2 SLat-ement. of t-ha Problem 

A kinematically redundant dual-arm manipulator is 
considered for peg-in-hole insertion type assembly where each arm 
takes a mating component. This assembly requires that the 
relative motion between the peg and the hole should be controlled; 
the absolute motion of one of the mating parts, say the peg could 
be anywhere in the workspace. 



23 


2.3 Description of the System and Position Kinematics 

Figure 2.1 shows two arms to be used in the assembly 
operation. A cartesian coordinate frame Y^, Z^) is taken as 

the reference frame (or world coordinate frame) for task 
description. The subscript 1 (1 = 1.2) will be used to denote the 
arm number, i.e. for arm 1 1=1 and for arm 2 1=2. Thus degrees of 
freedom of 1th arm is , i.e. for arm 1 and arm 2 the degrees of 
freedom are n^ and n^. respectively. For 1th arm a right-handed 
coordinate system ^10^ supporting base is 

established. The translational and rotational relationships 
between adjacent links are described by Denavit and Hartenberg 
matrix CFu et al., 19871. On each link at its joint axis a 

cartesian frame is fixed. The coordinate frame <X,., Y, . , Z.) 

corresponds to joint i+1 and is fixed to link i of the 1th arm. 
The Z^ axis is aligned with the axis of motion (rotary or 

sliding) of the ith joint of the 1th arm. The origin of the i th 
coordinate system is established at the intersection of Z^ ^ and 
Z^ j _2 axes or at the intersection of the common normal between 
the Z, . and Z, , axes and the Z,. axis. The X,. axis is 

1 1 1 i -1 1 1 1 1 

established along the common normal between the Z, , and Z,. 

1 i -1 1 1 

axes. The Y, . axis is assigned such that X, . , Y, . and Z, . form a 

1 1 1 1 1 1 1 1 

right-handed coordinate system. The last coordinate frame of the 
1th arm is (X, „ , Y, . Z, ). This frame is placed at the 

inj^ iHi inj^ 

centre of hand and is parallel to (X, i. Y, !• Z, ,). 

1 1 n^-l 1 

The Denavit and Hartenberg representation of a rigid 
link depends on four geometric parameters associated with each 
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link. These parameters are 0,d,a and ot. The angle is the 

joint angle between the axis and the Xj^. axis about the 

Zj . 2 axis using right hand rule. The distance from the origin 

of the i-1 th coordinate frame to the intersection of the 
axis and the X^. axis along the axis is denoted by cij j • 

The offset distance from the intersection of the Z^ axis with 

the Xj^ axis to the origin of the ith frame along the X^. axis 

or the shortest distance between the axis and the Z^,. axis 

is expressed by the parameter The angle . is the offset 

angle from the Zj^ axis to the Z^^ arms about the Xj^. axis 

using the right hand rule. For a rotary joint i, the parameters 
dj . , and remain constant and is the joint variable 

that changes when link i moves with respect to link i-1. For a 
prismatic joint ^1 i ' ^li joint parameters while d^. 

is the joint variable. 

The position and orientation of the ith link with 
respect to the i-1 th link of the same arm is expressed by the 
following Denavit and Hartenberg matrix CFu et al., 19873; 



(2.5-1) 

The description of the coordinate frame attached to the 

ith link of the 1th arm with respect to its base frame is 

1 0 

expressed by a matrix T , . as 

1 1 
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10 - _ 0 . 1 * 2 

1 i " IT^ 12 15* 


i-1 


1 i 


i j-1 

= n 
j=i 


for i = 1,2 ..n^ 


ri0n 

10 1 


ri0 10 10 

10 1 

*^11 

Pli 

— 

n, . s , . a , . 

1 1 1 1 1 1 

Pli 

o 

1 


1 

1 


(2.3-2) 


1 0 

where the 3x3 rotation matrix . gives the orientation of the 

ith coordinate frame with respect to the base coordinate frame of 

1 0 

the 1th arm. The 3x1 vector p^. is the position vector of the 

origin of the ith coordinate system with respect to the base 

10 10 


coordinate frame of the 1th arm. The vectors 
10 


nii. 


s , . and 

1 i 


a, . are the unit direction cosine vectors. These vectors in case 
1 1 

of end effectors frame are called normal vector, sliding vector 
and approach vector, respectively (shown in Fig. 2.2). The end 
effector frame of the 1th arm with respect to the world coordinate 
frame is given by 


w.. 


1 n, 




n, 


(2.3-3) 


where '^T,_ is the transformation matrix of the base frame of the 
1 0 

1th arm with respect to the world coordinate frame. 

For the 1th arm the joint vector is represented by an nj^ 
dimensional vector and thus the joint vector of the 1th arm is 


q 


1 



'12 


n 




T 


1 


1,2 


(2.3-4) 




Fig. 2.2 Normal .sliding and approach vectors of arm 1 
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where q^. denotes the ith joint angle position of the 1th arm. 

An assembly task with two arms is described by the 

relative motion of the end effector frame of arm 2 with respect to 

the end effector frame of arml . The transformation metrix of arm 

2 with respect to arml, denoted by T ,, is obtained from 

re I 




1 n. 


re 1 


W,j. 


2 n. 


The 


or , T 
matr i x 


re 1 
T 


re 1 


Wj -1 w^ <2.5-5) 

1 n^ 2 n^ 

is written in the partitioned matrix form as 


T 


re 1 


R , p , 

re 1 rel 

e 1 


(2.3-6) 


where R , and p , denote the rotational and translational 
rel '^rel 

components, respectively. The rotational matrix is expressed 

as a function of Euler angles 4> . , (b ^ and 0 _ in the form as 

^rl ^r2 ^r3 

given by <A.l) of Appendix A. This Euler angle vector 

[ ^ri • *^r5^ represents the orientation of the second end 

w 

effector with respect to the first end effector. Now T, _ and 

1 n^ 

'^T- are also written in the partitioned form as 
2 


W., 


1 n, 


w. 


w 


1 n. 


1 n, 


(2.3-7) 


w. 


2 n. 


w. 


'2 n. 


w 


2 n. 


(2.3-8) 


O 


1 
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Thus (2.5-5> yields 


R , ■ “rT „ “r, 

rel ^ 2 


and = R 


w^T 
1 n 


1 


w w 

^2 Hj 


The right hand side of <2.3-9) is expressed in 
direction cosines as 


1 n 


Wr 


1 


'2 n. 


n 

s 

a 

rx 

rx 

rx 

n 

s 

a 

ry 

ry 

ry 

n 

s 

a 

rz 

rz 

rz 


2. 4 Velocity Relations 

The end effector velocity of the 1th arm 


the six dimensional vector 
by 



w 


fi 


T 


T 


wh i ch 


w 


1 n , 


w 


n. 


Jl<qi) 


<2.3-9) 

<2.3-10) 

terms of its 


<2.3-11) 


is described by 
s related to 


<2.4-1 ) 
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where is 6 x manipulator Jacobian matrix relating 

the end effector velocity and the joint velocity and is discussed 

w 

in Appendix B. and is the angular velocity of the end effector 

of the 1th arm given as 


w 


Cl, = 


-.T 


w 


Ct> 


w 


lx 


<«> 


w 


ly’ 


(•> 


Iz 


(2.4-2) 


Referring to (B.7) of Appendix B the ith column of J^(qj^> is 
determined for the rotational joints by the following relation; 


J 


1 i 


<q 


> 


I-W 




w 


a, .,x(p, -p, .,) 

1 1-1 ^1 n, *^1 i-l 


w 


1 i-l 


(2.4-5) 


where '^R,„ is the rotational part of The relative angular 

10 10 

velocity vector is expressed as 

n , = fw , CO , « 1 (2.4-4) 

rel L J 

where the three components are along the x,y and z axes of the end 
effector frame of arm 1. This relative angular velocity vector is 
related to the angular velocity vectors of the two end effectors 
in the following way. 


As R 


re 1 


is orthogonal 

R , R^ , 
rel rel 


1 


(2.4-5) 


where 1 is 3x3 identity matrix. 


Differentiating both sides of 
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(2.4-5) with respect to time t, one gets 


d 

dt 




= O 


(2.4-6) 


where 0 is the 3x? null matrix. Now using (A. 10) of Appendix A 
(2.4-6) one can write CFu et al.,1987] 


‘rel dt 


Kel]= 


re 1 


r 0 

-CO 

1 

o>< 


CO 


rz 


rz 

0 


ry 


-CO 


rx 


-CO 


CO 


ry 

rx 
0 -J 

(2.4-7) 


ci T 

Using (2.3-9), the quantity “dT ^^rel^ expanded as 



w. 





d 1 

fWrjT 1 

1 ■'l nj 

= R. 

1 n j 

2 n 

dt 







w^ 


■1 n. 


+ "rT 

1 n 


1 


"r, 1 

1 n^ dt^_ 1 nj,J 


w. 


‘1 n. 


i n 


(2.4-8) 
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of (2.4-2) with the rotation matrix This yields 


For each arm, a relation similar to (2.4-7) is used to relate Ci. 


w. 




0 

“iz 

- CO, 

ly 

dt t T 

1'^rT = 

J 1 nj 

W 

“ "iz 

0 

w 

co- 

Ix 


w 

ly 

w 

- CO- 

lx 

0 


(2.4-9) 


and 


Wr. 


^2 n^ dt 


rw,jT dfw T -Iw T 

nj- nj ’*2 n. 


w 


w 


0> 


Zz 


CO 


w 

- (A> 


w 


2z 


CO 


w 


w 

"^2y “ ‘"2X 


2y 

2x 

0 


(2.4-10) 


Now combine (2.4-7) through (2.4 10) to get 


0 CO 

rz 

“CO 

ry 


0 

w w 

“2z ‘^Iz 

w , w 

- ^2y" ‘"ly 

“CO 

rz 

0 

CO 

rx 

= 

1 nj 

w , w 

- “zz*- "iz 

0 

w w 

‘"2X- ‘"lx 

“CO 

ry 

CO 

rx 

0 


w w 

L ly 

- “ex'" “lx 

0 


(2.4-11) 
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The rotation matrix R, is the rotation part of T (Z 

JL n X n 

and is written in terms of direction cosines as 


lx 

S, 

lx 

^Ix 



ly 

s, 

ly 

^ly 

• 

<2.4 

Iz 

®lz 

^Iz J 




Using (2.4-12) in <2.4-11) and comparing the elements of 
sides of (2.4-11) one gets 


Q 


re 1 


o> 


rx 


<A> 


ry 


CO 


rz 


= R 


t r 




< 2.4 


where 


R 


tr 


s. a. -s, a- 
ly Iz Iz ly 

a, n, -a, n, 
ly Iz Iz ly 

n- s, -n. s. 
ly Iz Iz ly 


-s, a, +s- a, 
lx Iz Iz lx 


-a, n, +a, n, 
lx Iz Iz lx 


-n, s- +n. s. 
lx Iz Iz lx 


s, a, -s, a. 
lx ly ly lx 

lx ly ly lx 

n, s, -n, s, 
lx ly ly lx 



5-3) 


12 ) 


both 


13) 
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•'tr ° '*1 n, 


(2.4-14) 


The relative linear velocity of the second end effector with 
respect to the first end effector is determined by differentiating 
<2. 3-10) with respect to time to get 


re 1 


d 

dt 


Wjj,T 


T Tw 

1 n,L' 


2 n, 


w 

- Pi 


n J) = n J h 


2 n 


w 

- P 


1 n ] 


-rT 

1 n. 









(2.4-15) 


w 

In order to expand the above equation, the vector p, and 

i n^ 

are written in terms of its elements as 

*'2 


• Plx 

Pi nj = Ply 

L Piz- 

w 

^2 n^ = 



(2.4-16) 


(2.4-17) 


Substitution of (2.4-9), (2.4-16) and (2.4-17) in (2.4-15) yields 
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"rT s “n, + "rT "p, -”p, 

1 rij p 1 ^ *^1 2 1 n. 


where S is a skew-symmetric matrix given by 
P 



(2.4-18) 


and the linear velocity of the end effector of the 1th arm is 
w * 

denoted by p, . Now the relative angular velocity vector from 

i nj 

(2.4-13) and the relative linear velocity vector from (2.4-18) are 
combined as 
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Where ^ 5,^5 

i s a 

5x5 

fuj ^ 

null 

matr i x . 

The 

end 

effector 

velocity 

vectors , 

name 1 y 

L”' 


"11 

u 1 = 1,2 

are 

related to 

the joint 

ve loc i ty 

vectors 

of 

the 

respect i ve 

arm 

through 

(2.4-1 ) . 


Therefore, the relative velocities can now be related to the joint 
velocities of both arms as 


re 1 


O 


re 1 


= J , q 

rel aug 


(2.4-20) 


where the augmented joint velocity vector q is 

3iVJ1 9 


aug 


^ [‘^11’ '^12‘“^l n^’ *^21 '’2 nj ’ 


(2.4-21) 


J .will be called relative Jacobian matrix and is given by 
re 1 




(2.4-22) 


where 


re 1 1 


1 n. 


3x5 


-rT 

1 n 


1 


-"r| 

1 n 


1 


(2.4-23) 


and 



<2.4-24> 
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As the prescribed rotation matrix <2.3-6) is in 
terms of Euler angles *^vZ' ^r3^ relative 
angular velocity vector in <2.4-20) is also expressed in 
terms of the rate of change of Euler angles as detailed in 
Appendix A. Thus <A.ll) of Appendix A together with <2.4-20) leads 
to 



<2.4-25) 


<2.4-26) 


<2.4-27) 


and the rotational matrix R 
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cos<?!>^j 

sin<;i>^jsin<^^2 

sin0^2 

-cos0^^sin0^2 

0 

cos<p^^ 


<2.4-28> 


2*5 Solution Procodur© 

In this work an assembly operation by a dual-arm 

manipulator is described by the relative path of the second end 

effector with respect to the first end effector. For kinematic 

computation, this relative path is defined by a sequence of knot 

points, i.e. at these knot points the six independent quantities 

p , and § , are defined. Next the transformation matrices T , 

'^rel rel rei 

are computed at these knot points using <2.5-6) and (A.l) of 

Appendix A. T , can also be expressed as a function of the joint 

vector q through <2.5-1) to <2.5-5) and <2.4-21). As q has 

^aug ^ ^aug 

ni+n 2 <> 6 ) elements, the number of unknowns is more than the number 
of equations and hence the system of equations has no unique 
solution, i.e. the inverse position kinematic solution is not 
unique and the system has redundancy of order n^+n 2 “ 6 . 

The kinematic redundancies prohibit a straightforward 
inverse solution. Therefore, this work follows the solution 

through incremental kinematics by imposing additional constraints 

on the kinematic behaviour. The following three kinematic 
criteria have been considered in this work; 

Case I. The norm of the joint velocity vector q at each knot 

3,(J[ Q| 

point should be minimum. 

The norm of the joint acceleration vector q' 


Case 1 1 . 


at each 
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knot point should be minimum. 

Case III. The man i pulab i 1 i ty at each knot point should be made 
large . 

The conditions for these three cases are now discussed 
sequant ial ly . Since the following procedures aim to solve for the 
joint velocities and joint accelerations only locally <i.e., the 
complete end effector paths are not used to derive instantaneous 
kinematic relationships), the velocity and acceleration terms are 
replaced by the equivalent incremental terms with the assumption 
that distance between the successive knot points is covered in 
unit time step. 

' T 

Case I: The minimum norm of joint velocity vector, i.e. q q 

3.U9 

is to be minimized <so that with lower joint velocity the 
required task space velocity can be achieved), where the 
joint space velocity vector is related to the task space 
velocity vector by (2.4-25). This problem is posed as 


minimize 


-T 

^aug aug 


(2.5-1) 


subject to 


re 1 


$ 


re 1 


= J q 

r aug 


(2.5-2) 


The above minimization problem is written in incremental form as 


minimize Aq Aq 

^aug aug 


(2.5-5) 



4-0 


subject to 


Ap 




re 1 


re 1 


= J Aq 

r aug 


<2.5-4) 


where Aq , Ap , and A$ , are incremental changes of q , p , 
^aug *^rel rel ^aug '^rel 

and § , respectively between the "current" knot point and the 

re i 


"next" knot point. 


-T 


Case II: The norm of joint acceleration, i.e. q q is 

cLLl^ 3.Li^ 

minimized because this may indirectly reduce the joint 
torques and velocity fluctuations. This minimization 
problem should satisfy the constraint equation relating 
the joint space acceleration and the task space 

acceleration. The acceleration constraint equation is 
obtained by differentiating <2.5-2). Thus the problem is 
posed as 

X 

minimize q q <2.5-5) 

aug aug 


subject to 


re 1 


i 


re 1 


= J q + J q <2.5-6) 

r aug r aug 


The expresions in <2.5-5) and <2.5-6) are written in incremental 
form as 

r r i 

<2.5-7) 


minimize lAq - Aq I jAq - Aq I 

j_ ^aug ^augp J [_ aug augp J 


subject to 


^*^rel '^^relp 


A§ - A§ , 
rel relp 


= J lAq - Aq 1 + AJ Aq 
r |_ ^aug ^augp j r ^aug 


<2.5-8) 
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where Ap , , A$ , , and Aq are incremental changes of p , , 

wnetB relp ^augp ^ *^rel 

5 and q between the previous knot point and the "current” 
rel aug 

knot point, and AJ^ is approximated as the change in at the 

current knot point and the previous knot point. 

Case III: The man i pu lab i 1 i ty is mainly used to avoid the dual-arm 
postures with kinematic singularity. The measure of 
manipul- ability is determined from the man i pu lab i 1 i ty 
ellipsoid. The man i pu lab i 1 i ty ellipsoid is described 
by the following equation CYoshikawa, 1984; Lee. 1989; 
Chiacchio et al. ,1991a]; 


(D 

1 

^ [j ]■* 

■-^Prel ■ 

L 

L r r 

A* 

^ re H 


From (2.5-9), the man i pu lab i 1 i ty of the redundant dual arm is 
defined as 


mp = -/ det[J^J^^] . (2.5-10) 

It is noted here that for a given p , and $ , , there are 
infinitely possible postures due to kinematic redundancy. One 
approach could be to search for those postures for which the 
manipulabi 1 i ty is maximum at each knot point. This gain in 
man i pu 1 ab i 1 i ty is obviously at the expense of large joint 
movements. Therefore, instead of maximizing the man i pu labl i ty 
alone, the criterion in this case is to have a larger value of the 
manipulabi 1 i ty while maintaining that the changes in joint angles 
between successive knot points should not be unduly large. This is 
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achieved by changing between successive knot points along 

the maximum change in man i pu lab i 1 i ty , where Aq is also 

^aug 

restricted by adding a penality on the joint displacements (as 
will be seen later in <Z.5-12)). The change in man i pu lab i 1 i ty can 
be described as a linear function of the change of joint vector as 



where mc(i) = ^ z-n- Thus me is the vector having n,+n„ 

«?q ( 1 > 12 

aug 

elements, and A<mp) is to be maximized while restricting Aq 

aug 

through (2.5-3) and subject to the constraint (2.5-4). Now 

intead of dealing separately with the above three performance 

criteria, a single minimization problem is posed from which all 

the three cases are derived. The objective function (2.5-7) and 

the constraint (2.5-8) reduce to those for minimum velocity norm, 

i.e. (2.5-3) and (2.5-4), respectively, for Aq = O, Ap , = 

augp re i p 

® AJ =0. Therefore, the minimum acceleration 

r e 1 p r 

criterion and the increased man ipulab i 1 i ty criterion are combined 
to get the f ol 1 owi ng mi n imi zat i on problem. 

minimize Z = f) fAq - Aq 1 fAq - Aq 1 -/? mc^Aq 

um u L aug augpj |_ aug ^augpj m aug 


(2.5-12) 
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subject to 


Ap , - Ap , 

^rel *^relp P 

= AJ Aq + J lAq - Aq 

- A§ ^ ^ 1 - ^augp 

rel relp 


(2.5-15) 


where, and /5^ are the weightages assigned to minimum joint 

acceleration and increased man i pu lab i 1 i ty criteria, respectively. 

Since the man ipulabi 1 i ty is to be increased, its sign is inverted 

in the minimization problem. For convenience of writting, a task 

vector t-r , is introduced as 
re i 


(2.5-14) 


Thus the constraint (2.5-13) is rewritten as 


Atr , - Atr , - AJ Aq -J lAq - Aq = 0 

rel relp r aug r I ^aug ^augpj 


(2.5-15) 


where, Atr 


relp 


relp 


relp 


(2.5-16) 


(2.5-17) 


For the minimization of the above problem the lagrangian is formed 


L = /3 I Aq - Aq 1 - Aq I me ' Aq 

‘ u I ^aug ^augp J [_ augp J m ^aug 
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Atr , - A-tr , - AJ Aq -J Aq - Aq 

rel - re ip r aug r I aug augp 


= 0 


(2.5-18) 


where X is the 
optimum values 
sat i sf i ed . 


undetermined multiplier vector. To determine 

of Aq the following canditions should 

aug 


the 

be 


aL 

aCAq 


aug' 


O 


(2.5-19) 


at 

ax. 


o 


( 2 . 5 - 20 ) 


Equations (2.5-18) and (2.5-19) yield 


2/9 fAq - Aq 1 ~ /^ me + AJ^X + J^X = 0 
' u L a.'J9 augpj ' m r r 

or. Fj^ + AJ ^Ix = /3 me - 2/9 [Aq - Aq | . (2.5-21) 

|_ r r J ’ m ' u [_ aug ^augpj 

Multiply both sides of (2.5-21) by (2.5-15) to 

obtain 


[ ^ = '’m [^r* “r]“= 

- 2/9 Fj + AJ 1 [Aq - Aq ] 

' u |_ r r J L aug ^augpj 

= /9 Fj + AJ \ me - 1(3 F^tr , - Atr , - AJ Aq 

'mj_r re ip r ^augp J 
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or, ^ = [C-’r * t-'r * ^ 


-1 


fj + AJ 1 me 
' m L r r J 


>4 


- 2/?.. Atr^^, - Atr , - AJ Aq 

•■ • rel relp r ^augp 


)] 


<2.5 


Combine (2.5-22) and (2.5-21) to get 


^ ^ ^r^T' K [^r " ^r] 


[ 


- Zft Atr - Atr , - AJ Aq 

u rei relp r ^augp 


= ft me - Zft Aq - Aq 1 
m *^1 3iug ^augpj 


Introducing J . as 

pi 


(2.5 


^ri = [^r " ] [t-'r + ^ 


-1 


(2 


the relation (2.5-23) is rewritten as 


J . 
r 1 


ft^ \j + AJ me- Zft Atr ,- Atr , - AJ Aq 
m j_ r rj L relp r ^augp 


= ft me - Zft lAq - Aq 

m L 3i'-'9 augp_ 


or , Aq 

aug 


r T 

r 

m 

1 - J AJ Aq + ^ 

1 - J . J AJ 


L ri rJ ^augp Zft 

r 1 r i r 


Im J y 

Imm m 



me 


- 22 ) 


me 


-23) 


5-24) 
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+ J . lAtr , - Atr 
ri I rel re 




(2.5-25) 


where 1 is an unit matrix of order n^^ + n^ . From (2.5-25) the 
value of Aq^^g for all the individual cases may be determined as 
fol lows : 

Case I : Minimum velocity norm is obtained by putting 


Aq = 0 , Atr , 

^augp re Ip 


= O , AJ = O , ^ = 0 , and ft ^ 

r ' m u 


Then (2.5-24) and (2.5-25) reduce to 


J . = J 
r 1 r 




-1 


(2.5-26) 


and 


Aq = J . Atr , 
^aug ri rel 


(2.5-27) 


where the right hand sides are determined from (2.4-26) and 
(2.5-17) . 

Case-II : Minimum acceleration norm solutions requires = 0 and 

ft 5»i 0 . Therefore, J is calculated from (2.5-24) and 
u r i 

the solution (2.5-25) yields 


Aq = fl - J AJ 1 Aq 

^aug r i rj ^ 


+ J . 
augp r i 


lAtr , - Atr , 1 

[_ rel relpj 


(2.5-28) 


Case-III : Increased man i pulabi 1 i ty solution is obtained by using 

Aq = O , Atr , = O , and =0. 

^augp re Ip r 

Use of these conditions in (2.5-25) implies that 


J . = J J J 
r 1 r r r 


] 


-1 


(2.5-29) 
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and 

- ^aue = \ 


me J . Atr , 
r 1 re 1 


<2.5-50) 


2.6 Results and Discussion 

This section presents the results for the inverse 
kinematics problem encountered in a peg-in-hole type assembly 
operation with a redundant planar dual-arm manipulator shown in 
Fig. 2. 5. All the manipulator joints are of revolute type. The 
associated transformation matrices and the Jacobian matrices for 
this dual-arm manipulator are detailed is Appendix C. 

Both the arms are indentical with the following 
dimensions (see Fig. 2. 5): 

a^i = a^j = 50 cm, a^^ ~ 3^22 ” ^15 “ ^25 ~ 

The assembly operation is specified by the relative path between 

the two end effectors <i.e., between the peg and the hole) which 

is inputed as T , (see (2.5-5)), where R , is computed from the 
re 1 re i 

specified orientations through Euler angles = § ,. The results 

re 1 

are presented for the following values of § , and p , : 

rel *^rel 


The orientation remains constant and is given by § 


re 1 


n rad , 
0 rad . 
0 rad , 


( 2 . 6 - 1 ) 


At the initial stage of assembly, “ 


70 cm . 
0 cm. 
0 cm. 


( 2 . 6 - 2 ) 
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‘10 

cm/ 



At the final 

stage of assembly, 

^relf 

0 

0 

cm. 

cm. 

• 

(2.6-5) 

Thus the two 

end effectors move 

towards 

each 

other 

covering a 


distance of 60 cm along the straight relative path. This relative 
path is approximated by a sequence of equally spaced 21 knot 
points. The starting point is designated as the zeroeth knot point 
and so the final point on the path becomes the twentieth knot 
point. Thus gets reduced by C5 0 03"** between two subsequent 

knot points and remains constant at the value given by 

(2.6-1). Thus at the kth knot point (k=0 . 1 , 2 . , . , 1 9 > , (2.5-17) 

impl i es that 

= C-5, 0,0.0,0,03^ . (2.6-4) 

and (2.5-16) implies that for the kth knot point (k=l , 2 . . . , 19 ) 

Atr , = C0, 0,0, 0.0. 03 ''' . (2.6-5) 

r e 1 p 

The inverse solutions (2.5-27), (2.5-28) and (2.5-30) also require 

the evaluation of J . , AJ and Aq at each knot point. J . is 

r 1 r ^augp r i 

evaluated with the help of (2.4-5), (2.4-22) to (2.4-24), (2.4-26) 

to (2.4-28), and (2.5-26). AJ and Aq are needed in (2.5-28) 

r augp 

and are evaluated from 

= J (k) - J <k-l) 
r r 


AJ (k) 
r 
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Aq <k> = q <k> - q (k-l> 

^augp ^aug ^aug 


k = 1 .2 .19 

( 2 . 6 - 6 ) 


The initial configuration of the dual-arm for the above 
mentioned assembly problem has been taken as 

20.38 deg. , c|j2" deg. , -122.1 deg., 

q 22 = 43.15 deg. , ^ 22 " 77.56 deg. , '^£5” ^8. 66 deg. 

The inverse kinematic solutions are presented for the following 
four cases : 

Case 1 : Minimum velocity norm solution from (2.5-27). 

Case 2 : Minimum acceleration norm solution from (2.5-28) with 

Atr ^ =Atr p as the knot points are equispaced. 

Case 3 ; Increased man i pu lab i 1 i ty solution from (2.5-30) with 

5 at each knot point along the relative path. 

Case 4 : Increased man i pu lab i 1 i ty solution from (2.5-30) where the 

factor ft /ft increases in the steps of 0.25 at each knot 
mu 

point starting with the value zeroeth knot point. 

Figures 2.4 to 2.6 show the obtained positions for 
joints of arm 1 for the four cases mentioned above and Figs. 2.7 
to 2.9 show the results for arm 2 . The slope of the position 
curves for all the joints corresponding to case 2 is seen to be 
nearly constant as the joint accelerations are being minimized. 
Also from the beginning to the end of assembly .the maximum 
displacements in the joints are for case 3. This is again expected 
since increasing man i pu lab i 1 i ty requires movements in the joints 
which do not contribute to the end effector movements. These large 
joint movements are avoided in case 4 by gradually increasing the 




2.4 Positions of joint 1 for arm 1 based on 
four performance criteria 



2.5 Positions of joint 2 for arm 1 based on 
four performance criteria 
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Positions of joint 2 for arm 2 based on 
four performance criteria 
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Fig. 2.9 Positions of joint 3 for arm 2 based on 


four performance criteria 
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weightage for increased man i pu lab i 1 i ty . 

Figures 2.10 to 2.15 show the joint increments 

j ; 1 ==1 1 2 ;i=l,2,5 ) for various joints of the dual arm. It 

is again evident that case 2 requires minimum variations in the 

joint increments. Maximum variations in the joint increments is 
for case 5, and this variation is reduced by using the criterion 
for case 4. Figure 2.16 presents the norm of which 

confirms that case 1 yields the minimum norm of Aq while case 2 

0lUC[ 

results in minimum variation in |Aq |. This is even more clear 

au g 

in Fig. 2. 17 which shows the norm of lAq - Aq I and evidently 

' aug augp • 

the minimum acceleration assembly operation is achieved for case 

2 . 

The man i pu lab i 1 i ty , as defined by (2.5-10), is plotted 
in Fig. 2. 18 for all four cases. The maximum manipulabi 1 i ty is 
obtained for case 5 but at the expense of large joint velocities 
and accelerations as seen in Figs. 2.16 and 2.17. A better 
compromise is obtained for case 4 which does result in increased 
man i pu lab i 1 i ty while avoiding undue large joint rates. 

Finally Figs 2.19 to 2.22 show the dual-arm configura- 
tions through various stages of the assembly for the four cases. 
In each figure the starting and ending configurations are marked 
by S and E , respectively. These figures do not reveal any new 
information but are more illustrative to confirm the observations 
made ear 1 i er . 

Similar exercises were carried out for various starting 
configurations with similar results, though the difference in the 
results for the four cases was not so pronounced for some starting 
configurations, following two main observations were common .- <i) 




Joint increment(radians). ^ joint increment(radians). 
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The minimum acceleration norm solution results in somewhat larger 
joint velocities (as compared to the results from the minimum 
velocity norm solution) but the fluctuations in the velocities are 
reduced greatly. <ii) If it is desired to include a criterion 
which increases man i pu lab i 1 i ty (so as to avoid the singularity 
configurations ) then it is better to take the arms to a more 
manipulable configuration in a gradual manner as the assembly 
progresses. An alternate approach could have been to reconfigure 
the arms at the starting configuration itself so as to have a 
large (or maximum) man i pu lab i 1 i ty ; but this requires very large 
joint movements. Therefore , unless the arms are already in singular 
configuration , the criterion discussed in case 4 helps to perform 
the assembly without large joint motions while gradually taking 
the arms to always a more manipulable configuration. 


CHAPTER 3 


CONTINUOUS AND DISCRETE DYNAMIC MODELS OF A DUAL-ARM 
MANIPULATOR CARRYING AN OBJECT 

3,1 Introduction 

In many industrial applications, robots have to handle 
objects which are beyond the capacity of a single arm manipulator. 
The difficulty in handling by a single arm may be due to odd 
geometrical shape, large dimensions or heavy weight of the object, 
A dual-arm (or even multi-arm) manipulator may then be 
conveniently employed for such operations. If the end effectors 
of the dual-arm rigidly grasp an object then the two arms together 
with the object form a closed chain. This chapter considers one 
such system where an object, while held rigidly by the two arms , i s 
to be carried along a specified path. For this problem, the 
trajectory planning is formulated in this chapter and the solution 
is presented later in Chapter 4, 

The statement of the problem is discussed in Sec. 5.2. 
For the selected dual-arm manipulator and the object. position, 
velocity and static force relationships are presented in Sections 
5.5. 5.4 and 5.5, respectively. The equations of motion for the 
closed-chain system are derived in Sec, 5.6. The redundancy in 
actuation is discussed in Sec. 5.7 and this redundancy is resolved 
according to minimum effort criterion and minimum electrical 
energy criterion. The problem of trajectory planning is posed in 
Sec 5.8 as a minimum time problem and minimum time plus minimum 
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energy problem. The numerical method, employed to solve the 
trajectory planning problem uses, the discretized set of 
equations. To this end, a discretization scheme is discussed in 
Sec. • 9 and all the necessary equations are restated in 
discretized form in the same section. Finally, all the relevant 
discrete functions and equations are linearized in Sec. 3.10. 


3.2 Stat-ement. of t.h© Problem 

The problem considers two serial link manipulators and 
an object is grasped between their end effectors. Each arm 
individually is an open chain mechanism consisting of 6 links. 
The arms grasp the object rigidly so there is no relative motion 
between the end effectors of the arms. Thus the arms together 
with the object form a closed chain mechanism which has 6 degrees 
of freedom. While carrying the object, all joints of both arms 
are considered to be active joints in the sense that each joint 
torque is to be determined independently. Thus the resultant 
force exerted on the object (which has 6 components) is generated 
by the 12 generalized joint forces of the arms and so the 
generalized joint force distribution is not unique. 

The problem undertaken considers the movement of an 
object from one point of the workspace to another point along a 
specified path with specified orientation. In other words, the 
geometry of the path is specified and the time histories of the 
object and manipulator along the path is to be determined. The 
torque limits of the joint actuators, the ranges of movement and 
the velocity limits of the joints are the system constraints. The 
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above problem is formulated in two steps. First the torque 
redundancy is resolved according to minimum norm of the joint 
torque vector and minimum actuator energy. Then, while satisfying 
all kinematic and dynamic constraints, the trajectory planning 
problem<i.e. determining the time histories of the joint 
velocities, joint torques ,etc.) is posed in two ways”l) minimum 
time problem, and 2) minimum time plus minimum actuator energy 
problem. It will be noted later that consideration of only 
minimum actuator energy leads to trivial results. 

3.3 Description of the System and Position Kinematics 

A fixed cartesian coordinate frame (or world frame) (X , 

w 

y , Z. > shown in Fig. 5 . 1 is taken as the reference frame for the 
w w 

task description. The subscript 1 is used to identify the two 

arms, where 1 = 1 for arm 1 and 1 = 2 for arm 2. A cartesian 

coordinate frame . , Y^.. Z^.) (1=1,2 ; i =0 . 1 , 2 . , . . 6 > is fixed 

to each link according to steps described in Sec. 2. 3. Following 

the same notations, '^T , „ (1 = 1,2) then describes the base frame of 

I (9 

1 0 

1th arm with respect to the world frame, T describes the end 

1 o 

effector frame of 1th arm with respect to that arm's base frame. 
Therefore, the end effector frames are defined with respect to the 
world frame through the transformation matrix where 

w w 1 0 

16 “ 10 16 


(3.3-1) 



dual-arm manipulator carrying an object 
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To describe the position and orientation of the object, 
a coordinate frame ^ attached to the object as shown in 
Fig. ?-l (henceforth, the subscript c will be used to refer to the 
object). The origin of mass centre of the 
object. The transformation matrix of the object frame with 
respect to the world frame is written as 


w. 


c 



(5.5-2) 


where is the position vector of the object mass centre with 

w 

respect to the world frame. R is the rotation matrix in terms of 

c 

Euler angles as defined in (A.l) of Appendix A. 

Now if describes the transformation of the object 

frame with respect to the end effector frame of the Ith arm then 


10-. w_ -1 w_ 6_ -1 

16 “ 10 c Ic 


(5.5-5) 


For the problems considered, all the three transformations on the 

1 0 

right hand side of (5.5-5) are assumed to be known and thus ^16 
(and '^T, from (5.5-1)) are also determined. 

The joint position vector of the 1th arm is denoted by 


^1 "" ^11 • ^'iZ 



(5.5-4) 


The inverse position kinematics can now be applied to obtain the 
joint position vector (the details isdiscussed in Appendix E) . 
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3.4 Velocity Relations 

The velocity of the object is described by the linear 

velocity vector '^v and the angular velocity vector , whose 

c c 

components in the world frame are written as 


w 


[ w w w 1 

V , V , V I 

cx cy C2 J 


and 


w, 


a 




w 


CO 


w 


cx cy 


CO 


cz 


1 ' 


<3. 4-1) 


C3.4-Z) 


The linear velocity vector is the time dertivative of the 
position vector in <3.3-2) i.e. 


w 


V 


c 



(3.4-3) 


The angular velocity vector *^0^ can be expressed in terms of Eular 
angles 4>^, their time derivatives 

following relation CFu et al . 19873; 


w 

cx 


0 

COS<^j 

s i n<^j 

sin<^2 


■e-. 

1 

w 

cy 

= 

0 

s i n0j 

-COS<^j 

sin^2 



w 

<» 

C2 


1 

0 

cos^2 



1 

1 


(3.4-4) 

The derivation of the above relation is given in (A. 11) of 

Appendix A. The end effector velocity of the 1th arm is denoted 

w T w T T 

in a similar way by the six dimensional vector [ ] . It is 

to be noted that Q-since the object is rigidly held 

cl 2 
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between the end effectors. The end effector velocity vector is 
related to the joint velocity vector through the manipulator 
Jacobian as 

= <qj> <5.4-5) 

where *^12 ^ six dimensional vector. 

Jj<qj) is called the manipulator Jacobian matrix which is in this 
case a 6x6 matrix. For rotary joints the i th column of the 
J^referring to <B.7) of Appendix B is given by 

Jli<qi> = 

<5.4-6) 


w_ r W 

1 i-l ^ ^ **16 


w 


**1 i - 1 ^ 


w 


1 i-l 


w 


L J 


3. S Static Forces 

The two arms interact with each other through the object 
held between the end effectors. The interactive force <see 
Fig. 5. 2) exerted by the object on the end effector of 1th arm is 
expressed in the form of a generalized vector as 


= r '^f 
1 '■ ^11 


w . 


12 


w , T 
• -^16^ 


<5.5-1) 


where the first three components constitute force vector and the 
last three components form the moment vector. Therefore, will 
a.lso be written as 
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^1 - [ ^If ' ^In J (5.5-2) 

where denotes the force vector and f denotes the moment 

vector. This implies that the force exerted by the end-effector on 
the ob j ect i s ^ . 

Due to the generalized force '^f ^ (1 = 1, E), there is a net 
force and moment vector at the centre of the object. Let - and 

C T 

represent the resultant force and moment vectors, 
cn 

respectively, at the object centre. Then the following relations 
hold (See Fig. 5.1 and Fig. 5.2) 


cf '‘ If 


w 


^‘Ef^ 


(5.5-5) 


W^ W W^ J. 

^cn ■ *^1 ^If >^2 ^Zf ” ^In ’ ^2n 


(5.5-4) 


with the moment arms and written as (Fig. 5.1) 


and 




w 

*'1 

it 

a 

- Pl6 

w 

w 

w 

•'z 

= Pc 

- P26 


(5.5-5) 


where, '^p and '^p w are the position vectors obtained from known 
c 16 

transformations '^T and , , of (5.5-5) and (5.5-1). 

c 1 o 

Equation (5.5-4) is in terms of vector cross products, 
whereas later the dynamical equations will be written in matrix 
form. Therefore, all the cross product terms are written in the 
fol lowing form: 
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i f a = b X c 


then 


Thus 


0 

b 


-b 


z y 
0 -b 


w 




■b b 0 

y X 


rl If 


(5.5-6) 


(5.5-7) 


w w- 

and x 


r2 2f 


where 


w. 


rl 


■ „ w w 

0 r. “ r, 

1 z 1 y 

w n, w 

- r, 0 r, 

1 z lx 

w w - 

r, - r, 0 

1 y lx 


(5.5-8) 


(5.5-9) 


w 

and S 


rZ 


0 

w 

w 


w w 

^2z " ’^2y 

^2z ® ^2x 


’^2y “ ^2x 


0 


(5.5-10) 


Now (5.5-5) and (5.5-4) are written in combined form as 


w. 


cf 


cnJ 


0 


1 

L ^ri 


w. 


If 


■ 1 

0 ■ 


2f 

1 n- 


'^S 

^r2 

1 


2n-' 


(5.5-11) 


where 1 is a 5x5 identitly matrix, O is a 5x5 null matrix, and S 
matrices are given by (5.5-9) and (5.5-10). In a compact notation 
form, (5.5-11) is written as 


“f = - j; "f, - "f, 

c Ic 1 2c 2 


(5.5-12) 
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and ^ 2 ^. are termed as the transmission matrices and these 

matrices relate the forces - and “'^^2 ^exerted by the end 

effectors on the object) to the resultant generalized force '^f at 

c 

the object centre . F i na 1 1 y for the static case, the end effector 
forces for the 1th arm are related to the respective joint 
torques • j s-s 

”^1 = j| 1 . (3.5-15) 

3.6 IJynainic Equat-ions of Arms and Object. 

The general equations of motion of any manipulator can 
be conveniently written through the Lagrange-Eu 1 er formulation. 
The direct application of the lagrangian formulation, together 
with the Denav i t-Hartenberg link coordinate representation. 


results in a convenient 

and 

compact 

algorithmic description of 

the 

manipulator equations 

of 

motion. 

To this end , 

f i rst 

the 

expressions for kinetic 

energy and 

potential energy 

are to 

be 

written. 






The kinetic energy 

of 

the 1th 

arm is expressed as 

CTourass i s 

and Neuman ,1985] 






6 

6 





"^1 = II 

I 

■’li '‘lij ' 1 ‘'I 



i=l 

i = 

1 





where, d^. .'s are the inertia coefficients given in Appendix D. 
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The dj,.j terms form the inertia matrix which is 

symmetric and positive definite. The potential energy of the ith 
arm is given by 

6 

Dtr V w T w 

PEj = - 2^ g Tj. (5.6-2) 

i = l 

where, '^g is the 5x1 vector representing the acceleration due to 
gravity with reference to the world coordinate frame (with the 
choice of world frame as shown in Fig. 4.1, '^g = [0 0 -9.81]"^), 
is the position vector of the mass centre of link i of the 
1th arm, the position vector being expressed in the world 
coordinate frame. Now the Lagrange-Eu 1 er equations of motion are 


dt 



dL, 




1,2 ; p = 1,2 6 (5,6-5) 


where, the lagrangian for the 1th arm is 


= KE^ - PE^ 


(5.6-4) 


and T 
This 


Ip * 


s the generalized force at the 
is written as 


P 


th joint of the 1th arm. 





w , 


1 i 


i=l 


(5.6-5) 
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where, the force exerted on the end effector, i.e. ^ . has been 
resolved on joints according to (3.5-13). Substitution of (3.6-1) 
and (3.6-2) in (3.6-4) and the resulting equation in (3.6-3) 
yields 



The dynamical equation (3.6-6) is used in this form 
itself while employing Finite Difference procedures (as will be 
explained later in Sec. 3.9). But for the sake of using easier 
forms of equations in this section. (3.6-6) is symbolically 


expressed 

i n the 

standard 

form as 



^1 ^ 

J 

j j 

1 = ^l^^l 

)qj + hj 

> + 

(3.6-7) 

where. 

i s the 

inertia 

matrix, 

is the vector 

i nvol V i ng 

centrifugal and 

Coriolis 

acce 1 erat i on 

and Cj is the 

pos i t i on 

dependent 

vector 

. The expressions for 

these quantities 

are not 


needed for the discussion. 

Thd arms interact with the object through the contact 

forces and as defined in (3.5-1). The object velocities 

are expressed by and in (3.4-1) and (3.4-2). Thus 

c c 
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Newton-Euler equation for the object are 


d 

dt 


(m 


c 




V ) 

c 




(3.6-8) 


and 


d 

dt 


('^I ) 

c c 




cn 


(3.6-9) 


where and are given by (3.3-3) and (3.3-4), 

w 

respectively. is a 3x3 inertia matrix and is obtained from the 

relation 


w. 


c 



(3.6-10) 


where. is the object inertia matrix in the object frame and 

IaJ 

is, thus, invariant, R is the rotation matrix part of T (a 

c c 

known transformation in (3.3-4)). 

Using (3.3-11) and (3.3-12), the equations of motion of 
the object, i.e. (3.6-8) and (3.6-9) are rewritten as 


d 

dt 

d 

dt 


(m 


('^I 



w 


- g 





(3.6-11) 


The dynamics of the closed chain of two arms alongwith 
the object is completely described by the relations 0.6-7} and 
0.6-11). As the interactive forces and between the arms 
and object are internal forces they can be eliminated to get a set 
of dynamic equations which relate the motions of object and arms 
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with the generalized joint forces. If and are nonsingular 

then from (3.6-7) the generalized contact forces and ^ can 

be written as CUnseren, 19913 

"fj - [Dj <qj> qj + hj (qj, q^ ) + c, (q^) - Tjj 

(5.6-12) 

and '^f 2 = ^ 2 ^ [^2 <<l 2 > + ^^2 ^*2^ ^ "=2 " ""z] ’ 

(5.6-13) 

Substituting these expressions for and ^ into (3.6-11) one 

gets 


d , w . 
-nr^ni v ) 
dt c c 


w w 
m g 
c 


dt c 


w 


a ) 

c 


T -T 
+ J, J ' 
1 c 1 


[Dj (qj) qj t hj (qj, q^ ) 


+ Cj <qj)j + J‘^ <q2> qj * (q^. 

+ (q^)] = Tj+ J-J Tj . 

The relation (3.6-14) is rewritten as 



(3.6-14) 



+ J 


T 

2c 



? = 0 


where. ? = + ?5 

with 






(3.6-15) 

(3.6-16) 

(3.6-17) 


(3.6-18) 
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+ Cj< q^) . (3.6-19) 




(5.6-20) 


and 


d , w . 

:;: . (m V ) - 
dt c c 


w w 
me g 


g (W 
L dt^ -^c 


w. 


O ) 
c 


(3.6-21) 


It can be noted that 

and 

^2 

are 

the 

joint torque 

vectors needed to move the arms 

as 

if 

the 

end 

effectors are 


unconstrained, and is the generalized force at the object 
centre needed to move an unconstrained object. 

At this stage, if it is assumed that the trajectory 
(i.e. the path, velocity and acceleration) is completely known 
then ? in (3.6-16) can be completely determined. Then (3.6-13) 
results in 6 equations in terms of 12 unknowns, namely six 
components each of and The following section discusses this 
redundancy in actuator torques and outlines two methods of 
resolving the torque redundancy. 


3«7 Resolution of Redundancy in Actuation 

The methods to resolve the torque redundancy are based 
upon optimizing some performance criteria . Here two performance 
criteria are d i scussed . The first performance criterion is to have 
the "minimum effort" 


by the actuators. This is achieved by 
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minimizing the sum of the weighted norms of the generalized joint 
forces. Thus, an objective function , to be minimized Csubject 
to constraint (3.6-15>), is written as 


^tbl '*’1 '*‘2 ^'tbZ “^2 


(5.7-1) 


where *^tb2 diagonal cost matrices and subscript 1 

and 2 correspond to arm 1 and arm 2, respectively. One way of 

defining the diagonal elements of the cost matrices C.^, and 

tbl tbZ 

is to take the reciprocal of the squares of respective joint 

torque limits. The significance of defining the cost matrices in 

this fashion is that the joint forces will be distributed such 

that the joint with higher load capacity shares higher load than 

the joint with lower load capacity i.e. the heavy joints will have 

low weightage and the lighter joints should get higher weightage. 

Thus the matrices C.. , and C.. - (based upon bounds on torques) are 

tbl tb/i 


written asC., , = diag 
tbl 


L ' T 

11 max 12max 


1 5max 


4- ] 

T , , J 


TT* ^ 

1 4max 1 5max ‘ 1 6max 


Sb2 = =^‘^3 


[ 7 


^ 21max ^ 22max ^ 25max 


(3.7-2) 


24max 


_1_ _L_ 1 

■ 


25max 


26max 
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where diag means is a diagonal matrix having the diagonal 

elements ' 2 etc. 

^llmax ^IZmax 

The second performance criterion is based upon minimum 
electrical energy consumption. As energy is the time integral of 
the power, the minimization of energy consumption means power 
should be minimized over the trajectory. In case of electrical 
motors the power depends upon three main parameters, namely c’^ the 
power supply resistance, c , the gear ratio between the motor and 
the joint, and c^ , the motor constant. If the motor torque is r 
then the power is given by CTan and potts ,1989] 

„ 2 r , g . m. 2 

Power = T c <c /c ) 


Now the objective function to be minimized (subject to 
constraint (3.6-15)} is written as 


= T . 


T 


'tpl 1 


^tp2 '^Z 


(3.7-3) 


where C, , and C. . are the cost metrices (based upon power 
tpl tp2 

consumption) for arml and arm 2, respectively. The cost matrices 
are written as 


'tpl 


diag ^tpl3 ^tpl6 ] 


Ctp 2 = diag 


[^tp21’ '^tp22' '^tp23 ''tp26 ] 


(3.7-4) 
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where . 

C. , . 
tpl 1 

and 

C. , . 
tp2 1 





<3. 7-5) 


r I' 

with Cj . and c^- as the power supply resistances of joint i of arm 

1 and arm 2, respectively. Similarly c^. and c^- are the gear 

ratios of the ith joint of arml and arm2. Lastly cT and c"’ are 

1 1 2 i 

the motor constants for the ith joint of arml and arm2. The above 
two performance criteria may be combined to form a composite 
objective function Z as 


Z ^2 ^2 


(3.7-6) 


where are weightages on the two performance criteria. Using 

(3.7-1) and (3.7-3) in (3.7-6) the objective function Z becomes 


where 


and 



^zl~ ^1 ^tbl ^2 ^tpl 


^z2 "" ^1 ^tb2 ^2 ^tp2 


(3.7-7) 


(3.7-8) 


In the redundancy resolution of generalized joint forces the 
objective function Z of (3.7-7) is to be minimized subject to 
satisfying the dynamic equations (3.6-15). To find out the 
optimum values of and the lagrangian L is formed as 
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L 


Z + X 




?> 


<?.7-9) 


where X is the 6 dimensional vector containing the Lagrange 
multipliers. To get the optimum values of and the 
lagrangian should satisfy the following conditions: 



(5.7-10) 



(5.7-11) 


dX 


= O 


(5.7-12) 


Combining (5.7-7). (5.7-9) and (5.7-10), one gets the optimum 

joint torque vector (for arml ) from 




* ^ T -1 -1 

or. T = - [C + c' ] ^ j/ J, X 

1 *• zl zl-* 1 Ic 


(5.7-15) 


Similarly from (5.7-7), (5.7-9) and (5.7-11), the optimum joint 

•Jf" 

torque vector is obtained as 



1^2 " 


h' ^Zc ^ 


(5.7-14) 


The relations in (5. 7-15), and (5.7-14) still contain the unknown 
multipliers X. To eliminate X, (5.7-12) is used as 
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T ^-T * , _-T * 

Jlc '^1 ^ -^20 *^2 ^2 - ^ = 0 


<?.7-15) 


Substitute t* and t* from (5.7-15) and (5.7-14) in (5.7-15) to get 


1 c 


IM' 


c - + c"*^, 

zl zl 




J, X 
1 c 


^ cL ] 


-1 


^2 ^20 ^ ® 


or , X = 


T 

J J 
1 c 


r [Si ^ <1 ] 


" ■ -Ic -L 1" 


2 2c 


-1 


(5.7-16) 


■¥: 

Finally and are obtained by substituting X from (5.7-16) in 


(5.7-15) and (5.7-14). Thus 



* 

■"l 

=^lred ? 


(5.7-17) 


* 

•"2 

=^2red ? 


(5.7-18) 

where 




T 1"^ -1 

[Sl " <1 j ^1 "ic 

^Ired 

[Si ^ <1 

] "ic 

1 c 1 


and 

C, 

2red 


+ J 


T 

2c 







(5.7-19) 


2c 





J 


Ic 
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^ ■'Ic \^zZ * ^Iz ] ■'z' ^Zc ■ <5.7-20) 

^Ired ^Zred matrices to resolve the joint torque 

redundancy and these matrices depend upon the motor parameters 
(which are constants) and the joint positions (which vary along 
the path) . 

As mentioned at the end of section 3.6 at this stage, 
is assumed to be known and hence and can be obtained from 

(5.7-17) and (3.7-18). But the problem specifies only the path, 
and the time histories (i.e. velocities and accelerations ) are to 
be determined. This part of the trajectory planning is addressed 
in the following section. 

3.8 Trajectory Planning 

The trajectory planning aspect involves determination of 
time histories of the joints in carrying the object through the 
specified path. One obvious consideration is that the task be 
finished in minimum time. Another consideration undertaken is that 
the overall energy used by the arms in performing the task should 
be minimum. To this end two objective functions are formulated as 
fol lows : 

Case I: Minimum Time Problem: The objective function for the 
minimum time can be expressed as 

t^ 

= J dt (3.8-1) 

0 


where t^ is the final time to reach the end of the path. 
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According to this criterion the total time of travel is most 
important so the system will try to be always at the maximum 
limits of actuator forces and actuator velocities whichever is 
reached first. 

Case II: Minimum Energy Problem:one might consider a minimization 
criterion based upon the electrical energy consumption. In that 
case, from relation (5.7“3), the total energy consumption can be 
written as 


tf 


j 

0 


tpi + ^r^tp2 


(5.8-2> 


where t| and are given by <5.7-17) and <3.7-18) and the cost 

matrices and are explained in <5.7-4) and <3.7-5). It 

may be noted here that minimizing the energy consumption alone 

over the trajectory does not yield practical and interesting 

results since the system behaviour is to move at very low 

velocities over a long time span and thus consume low power. 

Case III. Minimum Energy plus Minimium Time problem: For this 

(3.8-1) and <3.8-2) are weighted and combined to form the hybrid 

objective function Z. as 

te 


tf 


tf 


^te = X dt + J <^^r^tpl ^1 "■2'‘=tp2 ""P 


□ 0 

<3.8-3) 

where (3^ and are weightages of minimum time and minimum energy 
criteria. The trajectory planning problem is now stated as 
fol lows : 
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Minimize <5.8-l> for case-I and minimize (5.8-5) for case-II 
subject to the following constraints; 

i) The torques and satisfy the torque redundancy resolution 
(5.7-17) and (5.7-18). 

ii) While in motioni the actuators of the arms should not cross 
their velocity limits. These constraints are given by 


*1 imi n 


qii<t) 


1 imax 


imi n 


q2i(t) < 


imax 


i =1 . 


< t < t, 


(5.8-4) 


i i i ) Init ial ly the object is at rest and at the end of the 
trajectory it should come to rest . Hence the joint velocities 
of the arms should be zero at the begining and end of the 
tra j ectory . These constraints are given by 


q 


1 i 


(0) 



0 


'li 


(t^) 


‘2i 


(t^) = 


i = 1 


.6 


(5.8-5) 


iv) Every joint actuator has some joint force saturation limit. 
The trajectory planning should be such that no joint saturates 
during motion. The joint force constraints are given as 


T. . . 

1 imi n 

<- * 

- -"li 

(t) 

< 

1 imax 

^Z imi n 

* 

- -"Zi 

(t) 

< 

T 

Z imax 


1 6 ; 0 < t < t^ 

(5.8-6) 
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3.Q Discrete Tim© Formulations 

The dynamic equations of the arms developed in the 
earlisr sections are highly coupled and nonlinear. The closed form 
solutions of the dynamic equations are not possible. Hence some 
numerical method is to be employed to solve these equations. In 
the subsequent sections the numerical method used for solving the 
dynamic equations is discussed and these schemes follow the work 
in Tan and Potts C1989]. 

3.9. i DiscTotisation Sch&iro&s 

The dynamic equations of the arms and object are 
differential equations which involve position, velocity and 
acceleration terms of the joints and object .The Finite Difference 
Method is a numerical method where positions, velocities and 
accelerations are represented by discrete time functions. Thus the 
differential equations are converted to algebraic equations which 
are called difference equations. The process of generating 
discrete time functions from continuous time functions is called 
"discretization" which was developed from the principles of 
numerical integration CRice 1985]. The most popular 
discretization schemes for dynamic physical system modeling are 
the forward and backward Euler (or rectangular) algorithms and the 
trapezoidal (or Tustin) rule CPalm, 1983; Franklin, 1980]. The 
explicit forward and implicit backward Euler algorithms are easy 
to apply. The implicit trapezoidal rule is more accurate than the 
Euler algorithms. The trapezoidal rule is also called smoothing 
formula [Rice, 1985]. This formula has two following fundamental 
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propert i es : 


1 ) 

I n the limit 

(as 

the step-s i ze 

goes to zero). 

the 


trapezo i dal 

rule 

approaches the 

definition of 

the 


dervative C Franklin 1980]. 


2) the trapezoidal rule produces (at each sampling 
constant) a cubic spline function from which the 
position can be determined at any point within the 
sampling period [Jain 1979]. 

In the present work the discretization scheme uses the trapezoidal 
(smoothing) formula to approximate the differential relations. 

3.9.2 Discretization of Dynamic Models of Arms and Object 

The problem under consideration specifies the position 
and orientation of the object along a path for doing a task. The 
path is described by a succession of points in the workspace with 
proper orientation of the object. The specified path is divided 
into M intervals by choosing M+1 knot points along the path. By 
inverse kinematics the joint positions of the arms corresponding 
to each knot point are determined. The position, velocity and 

forces are functions of time in continuous dynamic model. These 
functions at different knot points are distinguished by their knot 
point numbers. Thus , the position for the 1th arm and the pth 
joint at the kth knot point is designated by q, <k) where 

1 j3 

M; 1 = 1,2; and p=l 6. Similarly is the joint 

velocity of the pth joint of the 1th arm and at the kth knot. If 

t^k), k=0 ,M, denotes the time when the object is at the kth 
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knot point of the path, then the time required for the arms and 
object to move from the kth knot point to <k+l) th knot point is 

At(k) = t<k + l) - t(k), for k-0 , and the corresponding 

reciprocal time interval is defined as 


= Atm 


1 

t<k+l) - t(k) 


for k=0 M-1 . 


<3. 9-1) 

If At<k-1> are known then q^pCk) 
(velocity of pth joint of 1th arm) can be determined by using the 
following trapezoidal smoothing formula: 


qip<k) - qip<K-l> = Y At(k-1> + q^p<k-l)) (5.9-2) 

Replacing At (k-1 ) by H(k) the relation (3.9.2) is rearranged to 

get the recursive relation for joint velocity q, (k) as 

ip 

q^pCk) = - q^p(k-l) + 2H(k)<q^p(k) - q^p(k-l)) 

for k=l .2, . . .M (3.9-3) 


or, q (k) 
Ip 


k 


/ 1 xk ' 
( -1 ) q 


Ip 


(0) + 2<-l)'^ ^ (-1)^ Aq^p( i-1 )H( i ) 
i^l 

; k =1 M (3.9-4) 


where Aq, (k-1 ) 
Ip 


q, (k)-q, (k-1) 

Ip Ip 


(3.9-9) 


F^or compact notations needed later ,(3.9-4) is rewritten as 
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M 

qip<k) = cv^p<k,0) +2 cVjp(k.i) H<i) (3.9-6) 

i=l 


; 1=1,2 P=1.2,...6 k=l,2,..M 


and 


where . 

cv , ( k , 0 > = 

Ip 


cv , <k , i ) = 
ip 


cv , < k . i ) 

ip 


(-1)*^ 9lp<0> . 

2 (-1)*^^^ Aq, (i-1) for 
Ip 

= 0 for i =(k+l ) . . . 


(3.9-7) 


i = l k 

(3.9-8) 

.M . 


3.9.3 Discr&t& Dynamic Equations for the Arn\s 

For discretization of the equations of motions, the 
Lagrange- Euler equation (3.6-6) is used. The first term of 
(3.6-6), which is the time rate of change of the momentum, is 
discretized as 



At(k-1 ) 


I 

i=l 


d, . (k) q, . (k) 

lip 1 1 


d, . (k-l> q, . (k-1) 
lip 1 1 
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^ = 1 M ; 1=1.2 . (3.9-9) 

In (5.9-9) dj.p(k) and cl^.p<l<-l) are discrete inertia coefficients 
at the kth and <k-l > th knot points. The continuous variable 
qii(t) is replaced by its discrete form qj^.(k-l) and 9 j^^k) at the 
(k-1) th and k th knot points. The second term of (3.6-6), which 
includes the coriolis and centrifugal effcts and is in the form 
of partial derivative with respect to the pth joint variable, is 
discretized as 



1 

2 


^li'k-1) 


Pi .(k) 


qii<k) 


qij'k-1) 


k=l , . .M 


(3.9-10) 


where the quantities dj.j(p;k) and dj.j(p;k-l) are hybrid discrete 
inertia coefficients CTan and Potts, 1989], and are defined as 


dj. j(p;k) 


li j 


q^jCk-l) . 


’ ■'^1 p-1 ^ ^ • 


qip(k). 


^ipfl 




(k) , 


.qi6<k) 


(3.9-11) 
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and dj . . <P;k-l ) = d 










. qj^<k) 


<5. 9-12) 


The discrete time functions in (3.9-11) and (3.9-12) are 
hybrid because in (3.9-12) p coordinates and in (3.9-11) p-i 
coordinates are evaluated at the beginning of the kth sampling 
period and the remaining 6-p (or (6-p+l)) coordinates are 
evaluated at the end of the sampling period. The third term of 
(3.6-6) is the effect of gravitational force on joint p of the 1th 
arm. and is a function of the joint coordinates. This term is 
discretized as 


dP£. 


PE^ (p;k) 


PEj (p;k-l ) 


dq 


Ip 




k = 1,2 M 


(3.9-13) 


In (3.9-13) the terms PEj^(p:k) and PEj(p;k-l) are hybrid discrete 
functions similar to the ones in (3.9-10). Thus the discrete terms 
PEj(p;k) and PEj(p;k-l) are defined as 


PE^(p;k) = PEj^ 


qjl(k-l) . 


Pi p-i^k-l). qip(k). 


'’l p+1^^^' 




(3.9-14) 


and PE^(p;k-l) = PE^ q^j(k-l) 




qjptk-l). 


•'l 




(3.9-15) 
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The first term on the right hand side of (5.6-6) is the 
geeralized joint force at joint p. This torque is discretized as 
a variable which is piecewise constant over the k th time 
interval. Thus the continuous variable Tjp<t> is discretized as 

t(k-l) < t < t(k) k = 1,2. ..M . (5.9-16) 

The second term on the right side of (5.6-6) is due to interactive 
forces with the object. This term is a function of joint 
coordinates. Therefore, the discrete form is 
6 6 

^'^lip'^^li ^ X! <3.9-17) 

i=l i=l 

where '3iip<l<) is the element of the pth row and ith column of the 
transposed Jacobian matrix of the 1th arm at the kth knot point. 
'^fjj(k) denotes the value of the intractive force at the kth knot 
point and is assumed to be piecewise constant like the joint 
torque T j . 


Now the complete form of the discrete dynamic model of 
the 1th arm is written by combining (5.9-9), (3.9-10), (3.9-15), 
<3.9-16) and <3.9-17) as 



1 


Aq^p(k-1> 


r o D 

dl,p<Pil<) - dj.p(p;|<-l) 

^ i=l j=l 



qj.<k-l) q^.(k) + qj.(k)qj.(l< 


= T 


IP 


'j PE, <p;k) - PE, <p;k-l) 
- 1 ) y + — t 

Jj ^ 

D 

(k) + ^ K = 1.2. ..M ; 1 = 1.2 ; 


P = 1 . 2 .... 6 


<3.9-18) 


where H(k) and q^^Ck-l) are as given by (3.9-1) and (3.9-3), 
respectively. The above equation is written in a compact form as 


MM M 

X! X ^ ^ ■ j 1 ^ H(il)H(jl) + V ctvOj^p(k, i 1 )H( i 1 ) 

• 1 1 • n • “1 Smmmmd 


il=l il=il 

+ ctco, (k) 
Ip 


il = l 


T <k) + 

Ip 


I 




^p(k)''’'fj . (k) ; 1 = 1,2 ; P=1.2..6 ; k=1.2..M, 


i=l 


(3.9-19) 


where, the expressions for qjp(k> have been substituted from 
(3.9-8) to yield the coefficaents in (3.9-19) as 


Ctmo^p(k, il . jl)= £: 


6 

Z Cd, . (k) cv, . <k; il )- 

1 ip 1 1 

i=l 


d 


(k-l)cv, . (k-1 . il); 
lip 1 1 
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Aq, (k-1) 
Ip 


6 6 


ZZ - ''up'P^K-d] 

i=lj=l j 


2 cv^.Ck l,il> cvj.(k;jl) + cv^.<k-l,ji) cv^.(k,il) 


+ CV^j(k.il) CVj.(k-l.jl) + CVj.(k.jl) CVj .(k-l.il)j 


for il < k and jl < k; 


and s = 


1 for jl = k 
1.0 for jl k, 


and Ctmo jp<k , i 1 . j 1 ) = 0 for il > k or jl > k 


(5.9-20) 


ctvoip(k,il) = ^Cdj.p(k) cvj^.(k;0) - d^ .p(k-l )cv^ . (k-1 , 0)) 

i =1 


Aq, (k-1) 
Ip 


6 6 


^ZZ h 


ip 


(p ; k) - d 


i=l j=l 


lip(p,K-l)] 


J cv^ . (k-1 . 0)cv^ . (k; i 1 ) + cv^.(k.0) cVj.(k-l,il) 


+ cv^.(k,0) cv^.(k-l.il) + cvj.(k-l,0) cv^.(k.il)j 


for i 1 ^ k 
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and ctvOjp<k , i 1 ) 


and e 

0 if il > k 


fl for = k 


0 for k. 


<3.9-21) 


ctco, <k) - 
Ip 


6 6 

hip'P'^o - '=iiip<PiK-i>i 
I i =1 j =1 ^ 


1 

i j^cv^ . (k-1 ,0) cv^.<k.0) + cvj.<k.0) cv^. (k-1.0)j. 


PEj (p:k)-PE^<p;k-l ) 

Aq, <k-l) 

Ip 


(3.9-22) 


In the above equation. Ctmo ^ p< k . i 1 , j 1 ) is the coefficient of the 
quadratic term H< i 1 )H< j 1 ) . ctvo^p<k . i 1 ) is the coefficient of the 

linear term H(il), and ctco^p(k) represents the constant terms in 
H's. 

3.9.4. Discr&te Dynamic Eq-uations for- th& ObJ&ct 

The position of the object at the kth knotpoint is given 

by the vector *^p <k), where '^p is as defined in (3.3-2). The 

c c 

orientation of the object at the kth knot point is given by the 
Euler angle vector C<;i>j(k). , ^^(k)]^ (see (3.3-2) and (A.l) 

of Appendix A). Now using the trapezoidal smoothing formula the 
linear and angular velocity vectors may be related to the 
respective linear and angular displacements vectors as 
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'^p (k) = j At(k-l) '^p (k) + (k-1) 

^ C u ^ C c 


<5.9-25) 


and ij<k) -$j<k-l) =jAt<k-l) §j<k>+ §j<k-l) . (3.9-24) 


The relations (3.9-25) and (3.9-24) are expressed in terms of the 
reciprocal time interval H(k) = 1/At(k-1) to get the linear and 
angular velocity vectors at the k th knot point as 


'^p (k) = - '^p (k-1) + 2H(k) a'^p (k-1) 
c c 


(3.9-23) 


and i<k) = - $(k-l) + 2H(k) + Af(k-l) 


(3.9-26) 


where 


A*^P (k-1) = '^P (k) - "^P (k-1) 
c c c 


(3.9-27) 


and A§(k-1) = $<k) - f<k-l) 


(3.9-28) 


The linear velocity vector of the object with respect to the world 
coordinate frame is 


W tAl 

V (k) = > <k) 

c c 


(3.9-29) 


By repeated use of (3.9-23) over k time intervals one gets 


'^v^<k) = '^p,,<k) = ^ (-1)^'*’'^ 2A'^p^(i-l) H(i) + (-l)'^ 

i =1 


; k = 1.2 


(5.9-50) 
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jhe equation (5.9-?0) is rewritten in the form 


w 


M 

v^<k> = <k.0) +1^ cv^ <k.i> H(i> 

i=l 


( 5.5 


where , 


cv ( k , 0 > 

cx 


<-1 ) V <0) 

cx 


" (-I) <i~l) for i < k 


= 0 for i > k 


<k.0) = (-1)*^ (0> 

cy cy 


cv <k,i) = <-l>^ 2A*^p (i-1) for i < k 

cy cy 


for i > k , 


cv (k,0) = (-l)*^ '^v (0) 

cz cz 


cv (k,i> = ZA'^P (i-l) for i < k 

W Am O 2» 


for i > k . 


<3.S 


Similarly (3.9-26) can be recursively applied to obtain 


-31 ) 


- 32) 
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ce^^(k,0) + ) <k,i) H(i) 


$(k) = 


■*■ y <=®r2 H(i} 

i -1 


03<K> 


ce^^<k.0) + J (k.i) H<i> 


= ce <k,0> + ) ce (k.i) H< i > 
c c 


(3.9-53) 


where 


ce^j (k.0) = (-1) <0) . 


ce^j (k.i) = (-1) (i-l). 


ce , ( k , 0 ) 
cZ 


(- 1 ) ( 0 ) . 
2 


ce „ (k.i) 
cZ 


(-l)*'*’*^ ZA<p (i-l). 


ce , ( k , 0 ) 
c3 


(- 1 ) <^^( 0 ) 


ce^j (k.i) = (-1) 2A<^^(i-l) 


(3.9-34) 


The components of the object angular velocity in the world frame 
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-jT 

3-nd the rate of change of Euler angles 

[*^1 *^2 "^ 3 ] related by (3.4-4). Therefore, if the 

instanteneous angular velocity vector is expressed in terms of the 
reciprocal time intervals by 


w, 


0 (k) 
c 


w 


« (k) 

cx 


w 


<0 (k) 

cy 


w 


CO < k ) 

cz 


cn (k) + ) 

cx i -1 


cn (k) + ’ 

cy . 4 ,^, cy 


cn (k, i ) H( i ) 
cx 

cn (k, i ) H( i > 


cn (k) 
cz 


M 


cn (k , i ) H( i > 
cz 


<5.9-53) 

then from (5.4-4) and (5.9-55) the coefficients in (5.9-53) are 
obtained as 

cn^^(k) = ce^^(k.0) cos<^j(k) + ce^j(k,0) sin<^j(k> sin<^2^*^^* 


cn (k,i> = ce ,,(k,i> cos<^, (k) + ce ,(k,i) sin<^,(k) sin<^,(k), 
cx c2 1 c5 1 2 


cn (k) = ce „(k,0) sin0,(k) - ce ,(k,0) cos<^, (k) sin<^j,(k), 
cy c2 1 c3 i 2 


cn (k.i) = ce ,(k.i) sin0,(k) - ce^,(k,i) cos<ii>,(k) sin<^.(k). 
cy c2 1 c5 1 2 


cnc2(k) = ce^j(k,0) + ce^j(k.0) cos</>2(k) 


and cn (k.i) = ce ,(k,i) + ce ,(k,i) cos^_(k) 

cz cl C) 2 


(5.9-56) 


Now the left hand side of (5.6-11) is to be discretized. The rate 


of change of linear momentum is written as 


dt 


m 


w 


V ) 

c 


w - 


m V 


c c 


(5.9-37) 


where is the linear acceleration of the object and is 

w 

expressed in the' discretized form as 


'^v^(k) = - '^v^<k-l )jH(k) 


M 

= cl^ (k.0) H<k) + ^ cl^Ck.i) H(i) H(k) 

i = l 


where 


cl <k.0) = cv (k,0) - cv (k-l,0> 

c c c 


and cl < k , i ) 
c 


= cv (k , i ) - cv (k-l,i) if i^ k 
c c 

=0 if i > k 


(3.9-38) 


(3.9-39) 


The discretization of the rate of change of angular momentum is 
done in the *fol lowing manner. 


~ hi 1 = H(k) hi (k) (k) 

cit c cj L ^ ^ 



'^l^(k-l) '^0^(k-l)j 
1.2 M (3.9-41) 


where. H(k) 


At (k-1 ) ’ 
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'^I^dO = ‘'ij- 0.6-10) 


'^R^<k> is the rotation part of in <3.3-Z) at the kth 

knot point, and '^a^<k> is obtained from (5.9-?5>. 

At this stage, the left hand side of (3.6-11) is also 
put in the form of (3.9-19) so that quadratic and linear terms in 
H's and the constant terms are collected. 

To this end, first (3.9-37) is expanded with the help of 
(3.9-39) and (3.9-32) to get 


m 


c 


% (k) 


g = 


M M 

crCj(k) + 2 crVj(k,il) H(il) + ^ 

il=l il=l 

M M 

crc2(k) + ^ crv2(k,il) H(il) + ^ 

il=l il=l 

M M 

crc^(k) + J crv^(k,il) H(il) tj 

il=l il=l 


M 

2 crmj(k, il, jl)H(il)H( jl) 
jl = l 

M 

2 crm2(k, i 1 , jl)H( il)H( jl ) 
jl = l 

M 

2 crmj(k, il , jl)H( il)H( jl) 
jl = l 


(3.9-41) 


where , 

crc^ (k) 
crvj (k , i 1 ) 



= m (cv (k,0) 
c cx 


cv ( k-1 , 0 ) ) 
cx 


if il = k 


0 


if i 1 ^ k , 
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crmj<k. il . jl) = m^<cv^^{l< , i 1 ) - cv^^<k-l , i 1 ) ) 
= 0 


m^<cVcy<k.0> - cv^y(k-l,0>) 

0 

crm2<k, il , jl > = m^< (k . i 1 ) - cv^y<k-l . i 1 ) ) 
= 0 


m^(cv^^(k.0) - cv^^(k-l,0)} 

0 

and crm, <k , i 1 . j 1 ) = m <cv <k,il) - cv (k-l,il)> 

> c cz cz 

= 0 


crc^ < k > 
crv^ (k , i 1 ) 


crc^^ k> 
crv^fk , i 1 ) 


Next the rate of change of angular momentum <5.9-40) 
with the help of <5.9-55) to yield 


d 

dt 



M 

<k) (cn <k) + y cn <k. i ) 
c ^ c L c 

i = l 


- <k-l) fen <k-l) 

c c 


M 

+ ^ cn^<k-l,i) H<i)) 
i = l 


or. 


d 

dt 





if jl = k 
if j 1 ^ k , 

if il = k 
if i 1 k , 

if jl = k 
if jl k. 

if il = k 
if i 1 ^ k , 

if jl = k 
if jl k. 
<5.9-42) 

is expanded 


H< i )) 
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crc 


M MM 

(k) +2 crv^<k,il) H(il) ^ crm^<k , i 1 , j 1 >H( i 1 )H< j 1 ) 

il=l il=l 


il = l 


M MM 

crc^<k> +2 crv^(k.il) H(il) ^ crm^Ck , i 1 , j 1 >H( i 1 )H( j 1 ) 


i 1 = 1 


il=l jl=l 


M MM 

crc^(k) +2 crv^<k.il) H<il> J crm^ <k , i 1 , j 1 ) H< i 1 }H( j 1 ) 


il = l 


il=l jl=l 


(5.9-45) 


where 


crc^<k) = crc^(k) 


crc^<k) 


crv,<k.il) = (k> cn <k> + *^1 (k> cn (k) 

4 cxx cx 


0 . 

cxy 


cy 



c 

u 

(k) - 

cxz 


cz 

Wi 

<k-l > 

cn ( k- 1 > 

cxy 


cy 


0 



cxx 


cx 


cxz 


cz 


if il = k 


if il k , 


crm , (k , i 1 , j 1 ) = (k> cn__(k,il) + cn^^/k,il) 




cxx 


cx 


+ 

<k) cn (k,il> ■ 

cxz 

cz 

- 

(k-l) cn (k-l. 

cxy 

cy 


= 0 

crv^ <k , i 1 ) = 

(k) cn (1 


cyx cx 

+ *^1 

(k) cn (k) - 

cyz 

cz 

- 

(k-l) cn (k-l) 

cyy 

cy 


w. 


cxx 

w 


cxy 

\ < 
cx 


cy 


I (k-1 > cn <k“l , i 1 ) 
cxz cz 

if jl = k. 
if jl ^ k. 


;) + <k) cn^^< 

cyy cy 

(k-l) cn <k-i; 
cyx cx 

- (k-l> cn^,< 

cyz cz 


107 


= 0 


if il = k 
if il k , 


crm^<k. il . jl> = cn^^<k.il) cn^y(k.il) 

+ "'cyz""> cn_.^(k.il> - ”l^y^<k-l) cn^^<k-l,Il) 

‘ " " "'Gyz***''’ cn^ <k-l,il) 


= 0 


if jl = k 
if il ^ k, 


crv^Ck.il) = '^I^^^<k) cn^^(k> + '^I^^y(k> cn^y<k) 
+ cn^^(k) - cn_ (k-l> 


C2Z 

czy 


cx 


■ cn^^/k- 1 ) - '^I __(k-l> cn (k- 1 ) 

czz cz 


cy 


= 0 


if il = k 
if il k, 


crm^(k.il. jl) = '^I^^^<k) cn^^(k.il) + cn^y<k.il) 

+ cn^^<k,il) - '^1^2^<k-l) cn^^(k-l.il> 


w 


I _^<k-l) cn (k-l,il) 
czy cy 


= 0 


w 


‘czz<'^-‘> cn^^<K-l.il) 
if jl = k 

if j 1 ^ k . 


( 5 . 9 - 44 ) 


w 


etc are the elements of the inertia matrix 


w. 


I <k) 

c 


g 1 ven 


by 


L 


J 


(5.9-45) 


"'l (k) = 
c 






(k) 

cxx 

(k) 

cxy 

*^1 (k) 

cxz 

(k) 

cyx 

w 

I (k) 

cyy 

(k) 

cyz 

(k) 

czx 

I (k) 

czy 

N 

N 

U 
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and cn^^(k) etc.are the elements of cn from (3.9-36). Now instead 
of writing the complete object dynamics (3.6-11) in discrete form, 
the combined arms plus object dynamics is considered, which is 
given by (3.6-15). As mentioned in Sec. 5.7, the torque redundancy 
arising in (3.6-15) is resolved according to (3.7-17) and 
(3.7-18) . 


3.9.5 Discrot& foTm. of Torquo R&dundancy Resolution 

The optimum torque distribution among the arm actuators 
is directly written from (3.7-17) and (3.7-18) as 

T* (k) = C, .(k) ?(k) ; 1 = 1.2 (3.9-46) 

1 1 red 

where,? (k) = (k) + ^ j, (5.6-16) to (5.6-19). 

(5.9-47) 


The explicit discrete relations for ?'s, which are six dimensional 
vectors, are written in terms of the components of ?'s. First 
considering ?j(k> and ^ , their components are expressed as 


(k) 




(k. 1 ) 


?^(k.2) . 


.?j(k.6) 


1=1,2, 


(3.9-48) 
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That is. the components of ?^(k) <1 = 1,2) are ?j<k.i2>.for i2 = 

1 , 2 . .6. Now from (5.6-17) and (3.6-18) and the left hand side of 
(3.9-19). the components of ?j(k) and ^^^k) become 

M M 

?j(k.i2) = J ^ ctmr^ .^(k.il.jl) H(il)H(jl) 

il=l jl=l 

M 

^ j ctvTj .^(k.il) H(il) + ctcrj .^(k) 
i 1=1 

1 = 1,2; i2 = 1 .2. . .6 (3.9-49) 

where 

6 

ctmri . 2 (k,il,jl) = ^ (k , i 2 . j 2 ) ctmo^ .^(k.il,)!), 

j2 = l 

6 

ctvri .^(k.il) = ^ j, (k , i 2 , j 2) ctvOj, .2(k,i2), 

j2 = l 


ctcri ^2^k) = ^ J^^(k,i2,j2) ctcoj^ j2^^^ 

j2 = l 


(3.9-50) 


ctm0j^ j 2 (k,il,jl), ctv0^ .^(k.il) and ctc0^ given in 
(3.9-20) and (3.9-22). Jgj(k,i2,j2) denotes the element in the i2 
th row and j2 th column of 

J ,(k) = jy <k) J7^<k) . (3.9-51) 
gl Ic 1 


In a similar fashion^ the six components of fj<k) in (?.9‘"47> are 


no 


written with help from 0.9-41) and (3.9-45) to obtain 

M M 

f^(k.i2> = ^ crm.2<k, il , jl) H(il)H<jl) 

il=l jl=l 


M 


^ 1 


crv.2(k,il) H(il) + crcj2^k) 


i2 = 1 ,2. . .6 . 


(3.9-52) 


Thus ^(k) in (3.9-47) is determined by adding ^^(k) and from 

(3.9-49) and ?^(k) from (3.9-52) and hence the optimum torque 
T^(k) is determined from (5.9-46) in the following form: 

M M 

T*(k,i2) = J J ctmj .^(k.il.jl) H(il)H(jl) 
il=l jl=l 


M 


ctv, .,(k,il> H(il) + etc. .^(k); 


+ 2 "‘-"1 ■ "'■''1 i2 


i 1 = 1 


1 = 1,2; i2 = 1 .2. . .6 


(3.9-55) 


where 


CTMj .^(k. il . jl) = 


2 *^1 red 


(k, i2, j2) i 


j2 = l 


ctmr j . 2 (k , i 1 , j 1 ) 


+ ctmr2 ■*" crmj2^k 


. il . ii)|. 


ctvi i 2 <K.il) = I ^^^(k.i2.j2) jctvr, 

j2 = l ^ 


j2<k. il) 
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+ ctvr^ .2<k,il) + crv.^ <k,il> | , 

ctci . 2 <k> = I jctcr^ .^(k) 

j2 = l l- 

+ ctcr^ + crc.^ <k)| . (5.9-54) 

Cl ^g^(k,i2,j2> is the element in the i2 th row and j2 th column 
of Cl <k) from (5.7-19) and (5.7-20) and Ctmri ^ ^ ^k , i 1 , j 1 ) , 

CtvTi j 2 <k, il) etc are from (5.9-50), (5.9-42) and (5.9-44). 

Finally (5.9-55) is represented as 

T* (k.i2) = h^CTMi ^2^<k'> h + h^ctVi .^(k) + ctCj .^^ik) 

1 = 1.2 i2 = 1.2...6 :k = 1,..M (5.9-55) 

where h is an Mxl vector such that its ith element, H( i ) , denotes 
the reciprocal time internal between the (i-l)th and ith knot 

points, CTMi i2^*^^ MxM matrix of coefficients of quadratic 

terms in H such that the element corresponding to the ith row and 
jth column of this matrix is CTMi .^(k.i.j) with the condition 

that CTMi = 0 if i>k or j >k, ctVi i 2 ^k) is an Mxl 

vector of coefficents of linear terms in H with a condition on its 
I th element that ctVj .^(k.i) =0 if i > k, and ctCi j 2 <k) 
1‘epresents the constant term. 
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3.9.6 DiscTGftt'S&di Tra.j&c tory Planning 

The trajectory planning problem which 

Sec. 5.8 may be converted to a discrete form by 

objective function and different constraints. 

restated as 

minimize where 

te 


is developed in 
discretizing the 
Thus the problem is 


M M 

hUT ^2 2 

i = 1 i = 1 

<5.9-56> 

subject to satisfying 

i) the joint torque dynamics of <5. 9-55), 


i i ) the 

constraints on 

joint velocity limits 

q 

1 imi n 

< 


i max 

imi n 

< 

q2i<k> < 

q_. forl< = l...,M i = 1....6 

2 imax 




(5.9-57) 

i i i > That 

at the begining and end of the trajectory energy 

joint should 

be 

at rest. 


Pli (0) 


= q^i = 

0 

q^- (M) 


= q^i = 

0 i = 1 6 (5.9-58) 

and iv) the 

constraints on 

joint torque limits 

■^1 • ■ 

1 imin 

< 

T* <k> < 

1 1 

^ 1 i max 

2 i mi n 

< 

T*.(k> < 

T-. fork= ; i = 1....6. 

2 imax 



f *T 

Ti 






H( i ) 


(5.9-59) 
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3,10 Linearization of Discrete Model 

The trajectory planning problem posed as above 
essentially attempts to find those values of H(k) , k= for 
which the function in (3.9-56) is minimum. The discrete 
dynamic models of arms and object developed in the earlier 
sections are nonlinear functions of the reciprocal time interval 
vector h. Thus the trajectory planning problem is an optimization 
problem which is nonlinear. In the next chapter the problem is 
solved by successive linear programming method, which requires all 
the functions and equations to be linearized about some nominal 
trajectory to get linear functions and equations. The objective 
function for the <j + l) th iterajtion is linearized about i.e. 
the h vector at the jth iteration. This is expressed as 


Z. (h^'*'^) = Z. (hU <5h) 
te t e 




dz. <h^) 

<5H^ (k) 


(3.10-1) 


k=l 


dH ^ ( k ) 


where 


(k) = (k) - H^k) 


(3.10-2) 


dz 


te 




M 

1 


il = l 


1 tpl 


* 

(il) 


and 


(k) 


CH^k))^ 


dH ^ ( i ) 
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+ (il> C 


^2 ^il) ^ 


(3.10-5) 


where from (5.9-55) 


(il.i2> 

dnUk) 


" 1 i2 jl)H<jl) 

il = l 


+ CTMj . 2 ( i 1 . jl . i )H( j 1 ) 


+ Ctv, 


for 1 = 1,2. 


(5.10-4) 


The relation (5.10-4) is also used to write the constraints on the 


torque limits as 


1 min - '^1 ^K) + y —L 


M - 

arj (k) 

\ J 6H ^ ( i ) < T , ; 

jt*! anUi) 


k = 1 ,2. .M 


(3.10-5) 


Similarly, the joint velocity vector at the (j+1) th iteration is 

linearized about the j th iteration and the velocity constraints 
are written as 


• . j ^ I ( k ) 

min - ^^'<) + y i 

^ -.1 . J , - . 


• i <5H ^ ( i ) < q , 

5 HJ(i) Innax 


1 = 1.2 ;k = 1 .2. .M-1 


from (5.9-58) 
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M - i 

•i V ( 

q;(M) + 2 ^ <5H’(i) = 0 


i =1 


^ ( i ) 


where, from (3.9-4) 


(3.10-6) 


^ ( i ) 


cvi . i 1 . i ) 


(3.10-7) 



CHAPTER 4 


COMPUTER SIMULATION RESULTS AND DISCUSSIONS 

4*1 Intproduction 

This chapter presents and discusses the results for 

three problems of trajectory planning. The system, i.e., the two 

arms and the object, remains same for all the problems and the 

problems differ mainly in specifying the base separation of two 

arms and the object trajectory. For problem 1. the object 

trajectory is a straight line path with its orientation remaining 

unchanged throughout the path. Problem 2 is an extension of the 

lem 1 where the object orientation is also changed along the 

vertical path. Lastly problem 5 considers a parabolic path for 

the object with orientation remaining unchanged . This trajectory 

selected to study a case where the velocities undergo 
reversal of sign. 

4*2 Problem Is Solution Procedure, Results and Discussions 

The first problem considers an object being lifted 
vertically up by the two arms. The object is initially at rest and 
gain brought to rest at the end of vertical trajectory. Each 

jectory planning problem is solved in two major stages. In the 

firsts^ 

all the kinematic and dynamic parameters are inputed 
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or evaluated. These quantities depend on the path geometry, 
geometric and inertial parameters of the object and arms. The next 
stage is to solve the minimization problem through successive 
linear programming. This stage is iterative and the quantities 
determined are functions of time. The steps (denoted by Sl.l, SI. 2 
etc.) of these two solution stages are described below in sections 
4.2.1 and 4.2.2, respectively. 

4.2. i Kinematic ccncl Dynamic Parameters 

Sl.l: Input parameters of the arms: Each arm is 
considered to be PUMA-560 manipulator (Fig. 4.1), whose kinematic 
and dynamic parameters are listed in Appendix F. These include 
the link parameters (length, twist etc.), inertial parameters of 
the links, joint velocity limits, joint torque limits, gear 
ratios, and motor constants. 

SI. 2: Input parameters of the object. The object is 
considered to be rectangular parallelepiped (Fig. 4. 2) with the 
specifications given in Table 4.1. 




F* * 

^•1 Establishing link coordinate system for a 
PUMA robot 
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2c 


- 1-0 0.0 0.0 0.0 

0-0 1.0 0.0 0.0 

0.0 0.0 - 1 .0 0.2 

0-0 0.0 0.0 1.0 


(4.2-4) 


pos 


SI. 4: Input th© object oafh « j. w 

ject path and compute Discrete 

itions of the object alonq the na-i-K 

iong the path- are described by the 


tranetormations t^<.> ,t the .th .not point, 
transformations are calculated from the specified obiect position. 

\<K>. and rotations *<., with the help of 1, .,-2, and (A.l> of 

Appendix A. For the first problem undertaken, the object path is 

spec i f i ed by 






W- 





0.6 

meter 

(0) = 

0 . 0 

meter 


0.6 

meter 


0.6 

meter 

(1) = 

0 . 0 

meter 


0.6015 meter 


0.6 


(k) = 

0 . 0 


m 

0.6 + 

.005 (k 


r 0.6 

meter 

(26) = 

0.0 

meter. 


meter 

meter 


k = 2 25 


0.7255 meter 



122 


P (27) = 
c 


0-6 meter 
0 • 0 meter 
0-725 me ter 


( 4 . 2 - 5 ) 


§(!<) = 


90 degrees 
90 degrees 
0 degrees 


- 0.1 27 


“154,50 degrees 

“78 . 50 degrees 

q,<0>= q„(0)= 15.26 degrees 

1 

155.09 degrees 
56.44 degrees 
_ 150.70 degrees 


( 4 . 2 - 6 ) 


The above description of ”p^(k) and *<!<> implies that the object 
i= being vertically lifted without changing its orientation. The 
total path length is 0,125 m and this path is divided into 26 knot 
points, l.e. M = 27. Except for the first three and last three 
Knot points, all other knot points are equispaced. Beginning and 

end points are more closely spaced to allow for maximum torque 
utilization. 


SI. 5: Compute the transformation matrices of the end 
effectors. The know transformation are (k). 

^2c- end effector transformation matrices are then 

Iculated at each point from (3.5-5). 
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SI. 6: Solve the inverse postion kinematics from Appendix 
E to obtain the joint postions vectors for two arms. Thus at each 
knot point. qj<k) and known. 

SI. 7: At each knot point, compute the A matrices for 
both arms from <Z.3-1). These are needed to evaluate Jacobian 
matrices and inertia coefficients. 

SI. 8: At each knot point, compute the Jacobian 

Jj{k) and J^^k) from <3.4-6), and transmission matrices 

J, from (3.3-11). 

Zc 

SI. 9: At each knot point, compute object velocity 
coefficients and constants from (3.9-32), (3.9-34) and (3.9-36), 
and constants and coefficients of joint velocities from (3.9-7) 
and (3.9-8) . 

S1.10:At each knot point, compute pseudo- inert iai 

matrices I , I , I etc and inertial coefficents d, . . from 
XX yy zz i i ) 

Appendix D. 

Sl.lliDefine weightages of performance criteria and 

T 2 ^ and compute the cost matrices for torque resolution 
0.7-8) . 


matr i ces 

J , and 
Ic 


from 
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S1 . 12 rCompute constants and coefficients of forces and 
torques applied at the centre of mass from (3.9-42) and (5.9-44) 
and also constants and coefficients of joint torques from 
(3.9-54) . 


4.2.2 Successi.x>e Linear Programming 

The linearized version of the trajectory planning 
problem was derived in Sec. 3. 10. This linearized problem is 
restated here as 

Minimize: Z (h^^^) - Z 

te 


te 


(h 


M 

) = y u 

k^l 


(k)6H^ (k) 


(4.2-7) 


where , 


} 


is the reciprocal time interval vector at jth iteration 


and its kth component is H^(k), 


and u ^ (k) 


ez. (hb 

te 

an ^ ( k ) 


(4.2-8) 


The objective function is defined by (3.8-1) for minimum time 

problem and by (3.8-3) for minimum time plus energy problem. 

Subject to: 

Equality constraints (velocities at the end of path are zero) 


M aq ^(M) 

q,^(M) + y iU <5H^<i) = 

i^l aH^(i) 


; 1 = 


0 


1.2 


(4.2-9) 
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Inequality constraints (limits on joint velocities and joint 
torques) 


*^lmi n 


< ^ <k) + 


3q ^ < k ) . 

“ ''imax ^ = ^'2 • 


i^l dH^Ci) 


(4.2-10) 


T < T 

Imi n 


*i 


(k) + 


! 

i^i 


dr * ^ ( k ) 

i S ( i ) 

^ ( i ) 


< T 


Imax 


1.2 ; k = l , . .M 

(4.2-11 ) 


The expressions and the associated terms in (4.2-7) to 4.2-11) are 
explained in Sec. 3.10. 

The optimization procedure is to start with some initial 
values of h vector with the iteration index j = 0, evaluates the 
variation 6h through the linearized problem mentioned above, 
update the h vector and repeat the procedure till optimum value of 
h is obtained. The associated steps are briefly described below. 


S.2.1:At the beginning of iterative procedure, the 
initialization is done with the following values; 

i) Every element of h® is taken to be 5 sec i.e. H (k) = 2 

S6C~^ ; k = 1.2..M. (M=27 as total 28 knot points are selected 

and the starting point is the zeroeth knot point). 
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ii) At each iterative stage, the upper and lower limits on the 

variation, <5h, are imposed, such that AH < ^H(k>< AH 

max min 

for k = 1,2...M. The values selected are AH =2 sec~^ and 

max 

^“^min ' starting guess for <5h is taken as 

<5H^(k) = 1.^ sec ^ for k = 1,2. ..M. 


i 


ii) The maximum and minimum allowable values of 
interval for every iteration is taken as H 

max 

u 1 -1 

H . =1 sec 


min 


reciprocal 
= 200 sec 


-1 


t ime 
and 


Now the following iterative procedure is adapted for the iteration 
index j = 0,1, till opt imum h is achieved. 


path < i . e 
i > 

i i > 


S.2.2: With the known values of compute, along the 

k=l .2 . . . .M> . 

joint velocities q,^(k) from (5.9-6) with q,^(0) = 0. 

Ip Ip 

Joint torques T*^(k) from (3.9-55), 

Ip 


i i i ) Cof f i c i ent u 


iv) coeffici ents 


(k) in (4.2-7) from 
dqj (k) 

i in (4.2-10) 

dH ^ ( i ) 


9t 


1 


in (4.2-11) 


(4.2-8) . 

from 5 . 10-7) , 

from (5.10-4) . 


v) Coefficients 


dH ^ ( i ) 
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o>q, ^ (M) 

S2.?: Compute the coefficients — L, ( 4 . 2 - 9 ) from 

aH ^ ( i ) 

(5.10-7). This is needed to satisfy the equality constraint at the 
last knot point. 


S2.4: The upper and lower limits on <5H^(k) (denoted by 
'^^low^*^^ ‘^'^upp^'^^’ respectively) are changed at every 
iteration subject to condition that these variations satisfy the 
bounds given in step S2.1 (iv). This is implemented as 


^ • '' = ‘'2 M 


(4.2-12) 


where 


<5H J (k> 
1 ow 


fAH . for H . - H^(k) < AH . 

min min min 




(4.2-13) 


H - H^(k) otherwise 
mi n 


<5H ^(k) = 

upp 


AH for H - H' (k) > AH 

max max max 


H - H^(k> otherwise 
max 


(4.2-14) 


S2.5: Solve the linear programming problem defined by 
^^•2-7) to (4.2-11). This was done using NAG subroutine E04MBF. 
Thus obtain the variations 6 H^(k>, k = 1 , 2 ...M at the j th 

i terat i on . 
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S2 . 6 : Evaluate (k) ,k = 1,2. ...M, i.e. the refined 

reciprocal time intervals, for next iteration with following 
conditions ; 

i) If in the linearized objective function, the difference 

between two iterations is Z. (h’+<5h^) -Z, <h^) (from 

t 6 “t © 

(^•2-7)) 2: 0 then in step S2 . 1 , new guess values for , 

0 

■^^max’ ‘^'^min’ taken and the program is 

restarted . 

ii) If Z^^ (h4<5hS - Z^g(hS (from <4.2-7))<0, then H^''’^<k> 
= H^(k) + 6H^(k>. 

S2.7: Compute the original nonlinear function Z^^ from 

(3.9-56). If - Z^g<h^> < 0 then go to step S2.2, else, 

if Z. -Z. <h^) >0 then assign AH = 0.8 AH and AH . 

te te max max min 

= 0.8 AH . . Now if AH < 0.1 or I AH , j<0.1 then terminate 

min max ‘ min' 

the program. Else go to step SZ.2. 

4,2.3 R&sxxltst with MiTximxirri Tim/& Criterion 

In this case the objective function is defined by 

(3.8“3) with the weightages ^ind The redundancy in 

joint torques is resolved by the method outlined in Sec. 3.7. In 

the minimum time case, the attempt will be to utilize the maximum 

torque capacity. To this end, the weightages in (3. 7-8) are taken 

2 = 1 and = 0. The cost matrices in <3.7-8), ^tbl 

c 

tb2’ determined in step SI. 11. 


and 



With the initial guess values of H®(k) AH ptr 

’ max 

taken in step SZ . 1 , the optimum values of reciprocal time 
interval. H <k), and optimum time intervals, At*<k) are listed in 
Table 4.2. These values correspond to the results obtained after 
70 iterations. 


Table 4.2 Optimum time intervals and reciprocal time 
intervals in minimum time case of problem 1 after 70 
i terat i ons 


k 

(interval number 
between kth and 
( k-1 ) th knot 
points) 

H*<k) sec ^ 

1 

140.54 

2 

150.51 

5 

125.58 

4 

125.99 

5 

122.41 

6 

120.85 

7 

119.21 

8 

117.57 

9 

115.91 

10 

114.25 

11 

112.52 

12 

110.78 

15 

109.02 

14 

107.25 

15 

105.41 

16 

105.56 

17 

101.67 


At*(k-1> = 1/H*(k) 


0.0071 
0 .0066 
0 .0080 
0 . 0081 
0.0082 
0.0085 
0.0084 
.0085 
. 0086 


0.0088 

0.0089 

0.0090 

0.0092 

0.0095 


. 0 


0.0097 

0.0098 









Table 4.2 (Continued) 



Thus the total time of travel, t* is 0.25 sec. ft* = ?At*<k> 

\ 1 -i k-l 

H*<k)J' presents the total time of travel versus 

the^iteration number. The initial guess values of H(k) equal to ; 
sec imply the total time of travel equal to ^.4 sec. 

As evident from Table 4.2. the optimum time intervals 
between equispaced knot points are not equal. It is to be notec 
from Sec. 4.2.1, SI. 4 that the distances between the first 
and the last three knot points are taken to be smaller than 
the distances between other knot points. In the trial runs it was 
bserved that if all the knot points are kept equispaced and if 
his distance between knot points is not very small then before 
d joint reaches its torque limit, the velocity limit is 
ched. Therefore, the torque capacity can be maximally utilized 
reducing the spacing between the knot points. Instead of 
ucing the spacings between all the knot points (which will 
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unduely increase the number of knot points and hence the size of 
problem) only the first two and the last two spacings. which are 
the important ones, are reduced. 

Starting with a guess value of equal reciprocal time 
intervals of 5 sec ^ between knot points. Fig. 4. 4 shows the 
obtained values of reciprocal time intervals, H^<k>, at iteration 
number j = 1 0 , 20 , 50 , 40 , 50 , 60and 70, where the abscissa represents 
the interval number k (k = l , 2 , . . . , 27) . The dots in Fig. 4. 4 
represent the H values and these dots have been joined only to 
show that these belong to the same iterative stage. Tables 4.5<a) 
and 4.3(b) list the same information in tabular form. 

Table 4.3(a) Reciprocal time intervals at intermediate stages 
in minimum time case of problem 1 


1 

1=10 
trave 1 
time = 

1 . 0883 
sec . 

j=20 
trave 1 
t i me = 
0 . 6046 

sec . 

j = 50 
trave 1* 
t ime = 
0.4189 

sec . 

j=40 
travel 
time = 
0.3210 
sec . 

j=50 
travel 
time = 
0.2703 

sec . 

j=60 
trave 1 
time = 
0.2563 

sec . 

j=70 
travel 
time = 
0.2531 

sec . 

1 

25.00 

45.00 

65.00 

85.00 

105.00 

117.41 

134.08 

■ 

25.00 

45.00 

65.00 

85.00 

105.00 

125.00 

145.00 

■ 

24.34 

43.45 

61 .22 

78.93 

95.84 

112.20 

123.67 

H 

25.00 

45.00 

65.00 

85.00 

105.00 

121.52 

124.01 

5 

24.51 

44.06 

64.06 

84.06 

104.06 

122.43 

122.45 

6 

25.00 

45 . 00 

65.00 

85.00 

105.00 

120.83 

120.85 

7 

24.60 

44.19 

64.19 

84.19 

104.19 

119.21 

119.21 

8 

25 . 00 

45.00 

65.00 

85.00 

105.00 

117.57 

117.57 

9 

24.52 

44.52 

64.52 

84.52 

104.52 

115.91 

115.91 

10 

25.00 

45.00 

65.00 

85.00 

105.00 

114.23 

114.23 

11 

24.69 

44.69 

64.69 

84.69 

104.69 

112.52 

112.52 

12 

25.00 

45.00 

65.00 

85.00 

105.00 

110.78 

110.78 
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Table 4.3<a> (Continued) 


13 

24.87 

44. 50 

64.50 

84.50 

104.50 

109.02 

109.02 

14 

25.00 

45 . 00 

64.52 

84.13 

105.98 

107.23 

107.23 

15 

24.71 

44.71 

64.71 

84.71 

104.71 

105.41 

105.41 

16 

25 . 00 

45 . 00 

65.00 

85.00 

102.87 

103.56 

103.56 

17 

24.67 

44.67 

64.67 

84.67 

101 .67 

101.67 

101.67 

18 

25.00 

45 . 00 

65.00 

85.00 

99.74 

99.74 

99.74 

19 

24.78 

44.52 

64.52 

84.52 

97.78 

97.78 

97.78 

20 

25.00 

45 . 00 

65.00 

85.00 

95.77 

95.77 

95.77 

21i 

24.57 

44. 31 

64.31 

84.31 

95.72 

93.72 

93.72 

22 

25.00 

45 . 00 

65.00 

85.00 

91 . 62 

91.62 

91.62 

23 

24.47 

43 .81 

63.81 

83.81 

89.47 

89.47 

89.47 

24 

25.00 

45 . 00 

65 . 00 

82.93 

87.26 

87.26 

87.26 

25 

24.15 

43 .33 

61 .09 

76.93 

84.99 

84.99 

84.99 

26 

25.00 

45.00 

65.00 

85.00 

102.33 

102.32 

102.36 

27 

25.00 

45 . 00 

65.00 

85.00 

98.94 

98 . 90 

99.11 


Table 4.3(b) Time intervals at intermediate stages in minimum 
time case of problem 1 


1 

j=10 
trave 1 
time = 

1 . 0883 
sec . 

j=20 
trave 1 
time = 
0 .6046 

sec . 

j=50 
trave 1 
t ime = 
0.4189 

sec . 

j=40 
travel 
time = 
0.3210 

sec . 

j=50 
travel 
time = 
0.2703 

sec . 

j=60 
travel 
time = 
0.2563 
sec . 

j=70 
travel 
t ime = 
0.2531 

sec . 

1 

0 . 0400 

0.0222 

0.0154 

0.0118 

0.0095 

0.0085 

0.0075 

2 

0 . 0400 

0 . 0222 

0.0154 

0.0118 

0.0095 

0.0080 

0 . 0069 

3 

0 . 0411 

0.0230 

0.0163 

0.0127 

0.0104 

0.0089 

0.0081 

■ 

0 . 0400 

0.0222 

0.0154 

0.0118 

0.0095 

0.0082: 

0.0081 

■ 

0 . 0408 

0.0227 

0.0156 

0.0119 

0.0096 

0.0082 

0.0082 
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Table 4.5(b) (Continued) 


6 

0 . 0400 

0.0222 

0.0154 

0.0118 

0.0095 

0.0083 

0.0083 

7 

0.0406 

0.0226 

0.0156 

0.0119 

0.0096 

0.0084 

0 . 0084 

8 

0 . 0400 

0.0222 

0.0154 

0.0118 

0.0095 

0.0085 

0.0085 

9 

0.0408 

0.0225 

0.0155 

0.0118 

0.0096 

0.0086 

0.0086 

10 

0 . 0400 

0 . 0222 

0.0154 

0.0118 

0.0095 

0.0088 

0.0088 

11 

0.0405 

0.0224 

- 0.0155 

0.0118 

0.0096 

0.0089 

0.0089 

12 

0 . 0400 

0 . 0222 

0.0154 

0.0118 

0.0095 

0 . 0090 

0 . 0090 

1? 

0 . 0402 

0 . 0225 

0.0155 

0.0118 

0.0096 

0.0092 

0.0092 

14 

0 .0400 

0.0222 

0.0155 

0.0119 

0.0096 

0.0093 

0.0093 

15 

0.0405 

0 . 0224 

0.0155 

0.0118 

0.0096 

0.0095 

0.0095 

16 

0.0400 

0.0222 

0.0154 

0.0118 

0.0097 

0.0097 

0.0097 

17 

0.0405 

0 . 0224 

0.0155 

0.0118 

0 . 0098 

0.0098 

0 . 0098 

18 

0.0400 

0.0222 

0.0154 

0.0118 

0.0100 

0.0100 

0.0100 

19 

0.0404 

0 . 0225 

0.0155 

0.0118 

0.0102 

0.0102 

0.0102 

20 

0.0400 

0 . 0222 

0.0154 

0.0118 

0.0104 

0.0104 

0.0104 

21 

0.0407 

0.0226 

0.0155 

0.0119 

0.0107 

0.0107 

0.0107 

22 

0.0400 

0.0222 

0.0154 

0.0118 

0.0109 

0.0109 

0.0109 

23 

0 . 0409 

0.0228 

0.0157 

0.0119 

0.0112 

0.0112 

0.0112 

24 

0 . 0400 

0 . 0222 

0.0154 

0.0121 

0.0115 

0.0115 

0.0115 

25 

0.0414 

0 . 0231 

0.0164 

1 

0.0130 

0.0118 

0.0118 

0.0118 

26 

0 . 0400 

0.0222 

0.0154 

0.0118 

0.0098 

0.0098 

0.0098 

27 

0 . 0400 

0 . 0222 

0.0154 

0.0118 

0.0101 

0.0101 

0.0101 


It is observed from Fig. 4. 4 and Table 4.3 that upto 
twentieth iteration, the H values have been simply increased while 
nnaintaining nearly equal time intervals. Then upto fortieth 
iteration except H(3) and H(25), all other H values still increase 
nearly equal amount, and there is an appreciable change in H(3) 
and H(Z5), As will be noted later, these are the intervals which 
separate the nearly uniform velocity regions from acceleration and 
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deceleration regions. After fiftieth iteration, the H values get 
successively refined with marked changes in the values. Finally 
the optimum values of H (k) are such that except at the beginning 
and the end, the H <k> values monotoni cal ly decrease along the 
path, i.e., the time intervals between equispaced knot points 
become larger implying slower motion (as will be seen later in 
Fig. 4. 11) . 

With the optimum time of travel, t equal to 0.25 sec, 
and optimum time intervals as given in Table 4.2- (corresponding to 
70th iteration), the joint velocities are plotted in Figs. 4. 5 and 
4.6, and the joint torques are plotted in Figs. 4. 7 and 4.8. It may 
be noted from (4.2-1) to (4.2-4) that the arm configurations are 
such that the object path is symmetric with respect to both the 
arms. Therefore the velocity and torque distribution is identical 
in the two arms . 

From Table F.3, the velocity limit on the third joint is 
2.1 rad/sec and from Table F.4 the torque limits on the first and 
third joints are 97.6 N.m and 89.4 N.m. Thus it is observed from 
Figs. 4. 5 to 4.8 that the minimum time of travel is constrained by 
the limits on first and third joint. At the starting of 
trajectory, the third joint exerts maximum allowable torque over 
the first time interval but the velocity limit is not yet reached. 
The third joint then still applies torque in the second time 
interval with a reduced value of torque such that the third joint 
Velocity limit constraint is not violated. By the end of second 
time interval, the third joint is moving at its maximum allowable 
velocity and thus it becomes the joint guiding the fastness of 
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motion along the major portion of trajectory. At the end of the 
trajectory, when all the joints have to reach zero velocity, the 
torque limit of jointl is reached. Eventhough the joint 1 is not 
moving, there is a torque applied by jointl which is a reaction 
moment needed to keep jointl stationary. 

It is noticed from Fig. 4. 5 and Table 4,3 that the time 
of travel does not reduce much after about 40th iteration. On the 
other hand, the individual time intervals between the knot points 
do change even after 40th iteration as shown in Fig. 4.4. 
Therefore, the information from Fig. 4. 3 may not be sufficient to 
conclude a suboptimal solution. This is further elaborated in 
Figs. 4. 9 and 4.10 which show the time histories of velocity and 
torque of the third joint taken at 20th, 40th and 60th iteration. 
(Since all the equality and inequality constraints are satisfied 
at every stage of iteration, the time intervals taken from an 
intermediate interation yield valid time histories, though not an 
optimal one). At Z0th iteration, the time of travel is still large 
(0.6 sec) and there are zagged fluctuations in the velocity time 
history. The time of travel is 0.321 sec at 40th iteration, 0.26 
sec at 60th iteration and 0.25 sec at 70th iteration. Though the 
time does not reduce much after 40th iteration (by about 28%), the 
plots in Figs. 4. 9 and 4.5 show an appreciable improvment in the 
velocity time history. There are still marked fluctuations in the 
velocity profile at 40th iteration which become smooth at 60th 
iteration. Likewise the torque profile in Fig. 4. 10 shows 
Fluctuations at 40th iteration which are eliminated at 60th 
iteration. The improvement from 60th iteration (Figs. 4. 9(c) and 


Joint velocity (rad/sec). joint velocity (rad/sec). 
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4 . 10 ( 0 ) to 70th iteration (Figs 4.5 and 4.7) is in taking the 
third joint torque to its maximum capacity in the first time 
interval, thereby reaching the velocity limit at an earlier time. 
The fluctuations observed at earlier stages of iteration are 
primarily due to the fact that the equality constraints on joint 
velocities (4.2-9) are to be satisfied. 

The object velocity along the trajetory is plotted in 
Fig. 4. 11. where the dots represent the vertical component of the 
velocity of object centre. Since the path is straight and 
vertical; all the other velocity components are zero. As noted 
from Fig. 4. 4 also, the object moves at slower velocity as it 
progresses along the path. The object velocity rises from zero at 
the begininning to a maximum value of 0.65 m/sec at the end of 
second time interval. Then the velocity gradually decreases to 
0.42 m/sec. whereafter it comes down to zero with a large 
dec 1 erat i on . 

The arms exert forces and moments on the object through 
the end effectors, which were denoted by and in (3.6-11) 
and Fig. 3. 2. These interactive forces and moments are shown in 
Figs. 4. 12 and 4.13, where the components are taken along x,y,z 
directions of the world frame. Comparision of Figs. 4. 12(a) and 
4.12(b) shows that, as expected, the forces by the two arms on the 
object balance along x and y dixections, and contribute equally in 
the z direction (which is the direction of motion). While the 
forces are large in the beginning, when the object is being 
accelerated upwards, the forces on the object are smaller at the 
end when the deceleration is assisted by gravity. The object does 
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4.1?<a) Interaction moments by arm 1 on 
in minimum time case for problem 



tirae(seconds) 

4.13(b) Interaction moments by arm 2 on 


object 

1 


object 


in minimum time case for problem 1 



. 14-6 


not execute any angular motion along the trajectory. Figs. 4. 15(a) 
and 4.1Mb) show that, for the major part of the path, the moments 
on the object about x and z bisections are zero, while there is a 
small bending moment about the y direction. At the beginning and 
the end. all the three moment components are present such that <i) 
there is a twisting moment about x direction. <ii) there is a 
bending moment about y direction, and (iii) both the arms exert 
6 gua 1 mo me nts about z direction which balance the mome nts produced 
by equal and opposite nonzero interaction forces in the y 
direct i on . 

The later Sec. 4. 2. 4 discusses the minimum energy 
criterion ,wi th the electrical consumption being defined (5.8-2). 
For minimum time case. being discussed here. the energy 
consumption versus the iteration numburis plotted in Fig. 4. 14. 
This figure shows that as the time of travel is reduced with 
increasing number of iterations, the energy first reduces with 
reduced time but later as time is further reduced towards minimum 
time, the energy starts increasing. This can be seen easily by 
noting that the energy is utilized in two main tasks. The first 
task is to support weights of the links and the object and this 
part of energy consumption will decrease with decreasing time of 
travel. The second task is to move the links and object and this 
part of energy consumption will be lower with lower velocities 
implying larger time of travel. Now it can be observed from Fig. 
4- . 1 4 that the energy required to move the dual-arm system starts 
becoming dominant after 26th iteration and thus the total energy 
consumption thereafter increases as the time of travel is reduced. 
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A . 2 . ? ■ P ^ ^ S iJ5.D ? SH Rsduc i nq the Computat i on Time 

As mentioned earlier, the results presented till now 

correspond to the initial guess value of H^(l<}=5 sec~^ 

(l< = l , 2 . . . 27 ) . AH^^^-2 sec and AH^.^ = -2 sec This choice of 

initial guess was made for two reasons. First, in the absence of 

any prior information .the values were taken to be on the 

conservative side for the "cold start" of the program. Second, the 

intermediate stages in convergence to the minimum time was desired 

to be studied. On the other hand, one could get informations from 

some trial runs to have the "warm start" of the program. To this 

end, the computation time can be reduced by (i>selecting higher 
0 

values of H <k>.and AH ,and <ii)reducing the number of knot points 
along the path. 

While keeping the same number of knot points (equal to 

28), various combinations of H^(k> and AH were tried. Large values 
0 

of H <k) and AH result in a much faster convergence but it 

requires a careful selection of the combination. To elaborate 

this, a higher value of H®(k> (same value for all intervals 

I<=1,2...27) needs a higher value of AH also, otherwise the proper 

<5h vector can not be obtained which will satisfy the end velocity 

constraints. This is evident from Table 4.2 also which shows that 
* 

H (k) varies significantly over the intervals and so large <5h is 
needed for higher values of H®<k). It was observed that with 

H®(k) = 85 sec~^(k = 1.2 Z7),AH^g^^= 2 sec ^ and AH^.^=-2 sec ^.the 

optimization program was terminated because within these bounds no 
feasible value 6h satisfies the velocity constraints at the end of 
the path. On the other hand, the initial bounds on AH cannot be 
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increased arbitrarily because of the fm, ■ 

. , following reason, a large 

permissible bound on AH allows <5h to be i 

large while solving the 

linear programming problem. Though th. . ■ 

a the linearized constraint 

equations are satisfied with thie i 

large value of < 5 h thp ,• 

'-'f the nonlinear 

constraint equations may be violated a 

-1 oiated. For example. with H®(k)= 8 ^ 

<'< = 1'2 27). AH =40 sec"^ = ^ 

max ®oo and AH . =-49 gg.-I 

... min ® . the 


sec 


. ‘ 4 . • , ^ mi n * tne 

.n. .a s eps of ta.an „sre ao large that after two iteratione 

the H<h, values Had almost converged to optl.u„ values hut the 
lo.nt torque 1 imUs „ere violated. The starting torque at joint , 

N.., and the torque at joint 1 at the end of the path „as 

found to be -99 s n m *. 

59.5 N.m (the torque li„it of joint 1 is 57.6 N 

Table 4.4 lists the final H(h, values for this case. It .as not 

Poss.hle to correlate the values of hounds on AH .ith the values 

Of H (K) hecause with sec-' AH„,„ = -4. sec’' .hlle the 


, min 

torque constraints .ere violated .ith H^h). 85 sec 


-1 


°Pti-i-ation procedure converged. „ithout 

'dolating any constraints, in three iterations with H®(k) = 65 

sec 

1^*^° iterations with H®(l <)=95 sec~^ 

27). These optimum H*(k) values are also listed in 

Table 4.4 and are seen to tally with the values in Table 4.2. 



Table 4.4 Optimum reciprocal time intervals tor different 

etartina H values in minimum time case of problem 1 


1 

2 

? 

4 

6 

7 

8 
9 

10 

11 

12 

13 

’4 

3 


sec”^ 


. .* 

H (k) 


H®<k)=89 sec~^ 


after two 
i terat ions 


143.23 
151 .65 
125.58 
124.01 
122.43 
120.83 

119.21 
117.57 
115.91 

114.22 
112.52 
110.79 
109.02 

107.23 
105.41 


H ( k ) =65 sec ^ 


after three 
i terat i ons 


140.54 

150.51 
125.58 
124.00 
122.41 
120.83 

119.21 
117.57 
115.91 

114.22 

112.52 
110.78 
109.02 

107.23 
105.41 


H (k)=95 sec ^ 


after two 
iterations 


141.26 

150.81 

125.58 

124.01 

122.43 

120.83 

119.21 
117.57 
115.91 

114.22 
112.52 
110.78 
109.02 

107.23 
105.41 


6 


103.55 


103.55 


103.55 


Table 4.4 (Continued) 


17 

101 .67 

101.67 

101 .67 

18 

99.74 

99.74 

99.74 

19 

97.70 

97.78 

97.78 

20 

95.77 

95.77 

95.77 

21 

95.72 

95.72 

95.72 

22 

91 .62 

91.62 

91.62 

25 

89.46 

89.46 

89.46 

24 

87.26 

87.26 

87.26 

25 

84.99 

84.99 

84.99 

26 

102,78 

102.56 

102.59 

27 

100.08 

99.15 

99.19 
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The next attempt was to reduce the number of knot 
points. Since the minimization problem is subject to twelve 
equality constraints, given by (4.2.9), at least thirteen 
intervals (i.e., fourteen knot points) are required on the object 
path. A set of equispaced 14 knot points along the path length of 
0.125 m (see aiso(4.2-5)) will imply large spacings between 
beginning and end knot points and hence the joint torque 
capacities will be underutilized as has been shown earlier in this 
section. Therefore, taking clues from the results discussed 
earlier, the following set of 14 knot points was selected. 

0,6 meter 


w 


p (0) 


0,0 meter 


0 . 6 meter 




15 


® • 6 meter 

< 1 >= 0.0 meter 

0.6015 meter 


p <k ) 
c 


® ® meter 

meter 

0.615 + 0,010(}< — 2 ) meter 


^'or k = Z 6 . 


P^<k)= 0.0 


meter 


meter 


0. 670 + 0. 010<k-7) meter 


0.6 meter 


^ ^ ^ ® ■ ® meter 
0.7255 meter 


for k=7, . . .11 


0.6 meter 
p^<15>= 0.0 meter 

0.725 meter 


(4.2-14) 

The initial guess values of H®(k> (k = 1.2 15) were 

3-lso based upon the earlier results. Instead of taking equal 
values of H <k) for each interval, as was done in all previous 
cases, now the program was "warm started" with the following 
values of H®(k) listed in Table 4.5. 









153 


Table 4.6 (Continued) 


2 

38.54 

3 

60.81 

4 

56.10 

5 

54.52 

6 

55.82 

7 

35.73 

8 

51.30 

9 

49.38 

10 

47.37 

1 1 

45.26 

12 

27.24 

13 

98.87 


It is to be noted that the minimum time of travel with 
14 knot points is 0.26 sec while the same with 28 knot points was 
0.25 sec. This slight increase in time is due to the fact that the 
second time interval is larger now and as noted in Table 4.5, the 
third joint velocity limit is reached at the end of this interval. 
Taking two more knot points so that the second and the 
last-but-one intervals are shorter (i.e., with total number of 16 
knot points) reduces the minimum time to 0.25 sec. 

The computations were performed on the Hp 9000/850S 
machine. The computation time for each iteration was on average 7 
min. with 28 knot points and 1.25 min. with 14 knot points. 

4,2.4 Results with Miriirrvum En&rgy Crit&rion 

The objective function is defined in this case by 
<3.8-5) with = 0 and ^2 = torques are also 




resolved according to minimum power consumption so that in 

(}.7-8). the weightages are = 0 and r^ = l. The cost matrices in 

(5.7-8). and • are determined in step SI. 11. The initial 

guess value of H<1<} in step S2.1 was taken as 2 sec"^ implying the 

guess value of time of travel as 15.5 sec. The bounds on AH were 

taken as AH = 1 sec ^ and AH^. = -1 sec"^ . 
max min 

The variation of energy consumption with iteration 
number is shown in Fig. 4.15. Compared with Fig. 4.14, the energy 
consumption in Fig 4.15 continuously decreases in successive 
iterations because the objective function is based upon energy 
criterion. Figure 4.15 also shows that convergence is fast in 

the beginning but very slow towards the end. The variation of 
time of travel with iteration number is shown in Fig. 4.16. 
Even with low starting H<k), energy reduces from 52.61 Joules to 

4.25 Joules and time of travel reduces from 15.5 sec to 1 sec in 

merely 11 iterations. In the later stages of iterations, the 
convergence is so slow that after 70th iteration energy is 2.98 
Joules (time of travel = 0.71 sec) and after 150th iteration 
energy is 2.68 Joules (time of travel = 0.62 sec). 

As done in Sec. 4.2. 5.1 for minimum time case , warm 
start was also tried for minimum energy case with various guess 
values of H®(k) and bounds on AH. The only saving thereby obtained 
W9-S in the initial stages of iterations , the convergence in the 
later part being always very slow. The mininrium energy problem 
inherently requires much larger number of iterations as compared 
to minimum time problem when the method of successive linear 
programming is used to solve the nonlinear optimization problem. 



Fig. 



4.14 Variation of energy „ith iteration number 
mirnmum time case for problem 1 


in 



Fig. 4.15 Variation of energy with iteration number in 


minimum energy case for problem 1 


time of traveKsecs) 




Table 4.7 (Continued) 



H<1<) is more or less uniform. 


The joint velocity profiles are shown in Fig. 4 . 17 and 
4.18. Comparing these with Figs. 4.5 and 4.6, it is seen that the 
joint velocities are now much lower but slightly irregular in 
nature. Minimization of energy alone is expected to lead to low 
velocities and low accelerations . In fact, if the gravity loading 
was not to be supported by the joint torques then an extremely 

slow movement over a large interval of time will lead to lower 
ve1u6s of ©norgy. 

The joint torque profiles are shown in Fig. 4.19 and 
4.20. The torques are much lower than in Figs. 4. 7 and 4.8 but 
somewhat fluctuating. Finally, the velocity profile of the object 
is shown in Fig. 4. 21. Here again the velocity is low compared to 
'"‘9. 4.11, but 


small fluctuations in the velocity are present. 
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4.20 Torques of joints 4,5 and 6 of both arms 
with minimum energy case for problem 1 
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Fig. 4.21 Linear velocity of object in minimum energy 
case for problem 1 




162 


A combined obiectivp ■fi..^^ 4 .- 

wujeotive function with i*jo;r,t, 4 . 

" weightages on both 

minimum time and minimum energy was u.ph * 

find the time 

histones for the same problem. it wac ^ 

was found that with this 

criterion the convergence was always slow in the later 

une later parts of 

iterations and «arnr, start also did not help in reducinc th 

H reoucing the number 

of iterations by any significant amount. 

The results for two cases in this Mfor, 

his category are presented 

here. The first case corresponds to ^ = y - 0 s a w 

- 0.5 and /5 = /? = 

in <3. 7-8) and <3. 8-3). respectively. These weights for the 

econd case are taken as = U . 5 and r, - p. . 0 . 1 . 

o -1 ... -2 

■ . 
min 

i, wil 

25 sec . the initial convergence was 
very fast but the total number of iterations uere still near 180. 
The optimum reciprocal time intervals and the objective functions 
for the above two cases are listed in Table 4 . 8 , the joint 


max 


^*^max " ^ , AH . 

^ min 


= 

^2 = 

0 . 1 . 

Wi th 

-2 

sec ^ , 

the 

number 

H® 

(k) = 

20 

sec ^ , 


-1 


above two 


cases are listed 



Table 4.8 Optimum reciprocal time intervals r 

time-energy case of Problem 1 ^ minimum 


Case 1 

= ^2 = 0.5 

= /?2 = 0 -^ 
Optimum travel time 
= 0.57 sec . 

Optimum energy 

= 2.57 Jou 1 es 

Objective function 
= 1.57 


?'l = 0.9. 


Case 2 

• r, = 


Pi = 0-S. = I 

Optimum travel time 

= 0.44 SQQ 

optimum energy 

“ 2.46 Joules 

Objective f unct i on 
= 0.64 


H*<k) sec“^ 


II* - 1 

H (k) sec ^ 


51 . 40 
55.56 
50.10 

53.27 
54.93 
58.18 
60 .25 
60.22 
58 . 00 

54.28 
50 . 40 

48 . 02 
46.48 
45.26 
44.34 

44.03 
44.60 
46 . 58 
49 . 54 
51.22 


61.41 

67.74 

63.93 

73.36 
78.54 
81.31 
80.71 

77.36 
73.80 
70.33 
67.48 
63.01 
54.87 
48.66 
50.27 
56.20 

62.30 
66.75 

69.31 
68.56 
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Table 4.8 (Continued) 



of problem 1 velocities are plotted in Figs. 4, 22 and 4.23, the 
joint torques are shown in Figs. 4.24 and 4.25, and the object 
velocity is shown in Figs. 4.26 and 4.27. For case 1, with (3^ = 
/?2 = 0-5. the objective function is more dominantly guided by the 
energy term and therefore except for a marginal reduction in 
travel time, the velocity and torque profiles resemble those in 
Figs. 4.17 to 4.21. The case 2 with (3^ = 0.9 and (3^ = 0.1 has 
equal numerical contributions from minimum time and minimum energy 
terms but the velocity and torque profiles still have trends like 
those in minimum energy case. 

It is to be noted that contrary to the expectations, the 
energy has decreased with decreased weightage on the minimum 
energy term so that f3^ = 1, 0.5, 0.1 result in values of energy as 
2.65 Joules, 2.57 Joules and 2.46 Joules, respectively. No fixed 
pattern or trend, however, could be seen while taking results with 
many other combinations. 




O 192 

09 

03 

S 128 

GO 

L. 


064 / 


g 000 
0) 

> -m 



000 a06 002 OiS 024 030 036 a« a48 054 flyeo 

timeCseconds) 
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Fig. 4.25<a) Torques of joints 1,2 and 3 for both arms 
with minimum time-energy case for 
problem 1 (case 2) 
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^■19. 4.23(b) Torques of joints 4,5 and 6 of both arms 
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4.3 Problem 2: Results and Discussions 

Tha problen. 2 is sn extension of the problem 1 where 
slongwith the vertical translation the object undergoes rotations 
also. The object is initially at rest and is again brought to 
rest at the end of the trajectory. The steps of two solution 
stages are as detailed for problem 1. and differ only in terms of 
the input parameters. The Important input parameters for this 
problem 2 are listed below, where the step numbers refer to those 
in Secs. 4.2.1 and 4.2.2. Other Input parameters are same as in 
problem I. Input the object path and compute “t_.. For the problem 
2, the object path is specified by 



< 0 ) 


0 . 6 meter 
0 . 0 meter 
0.58 meter 



( 1 ) 


w 

P^(K> 


0 . 6 

meter 



0 . 0 

meter 



0 . 5825 

meter 



0.6 



meter 

0 . 6 



meter 

0.585 + 

0.005 (k-2) 

meter 


for K = 2, . . .25. 
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(26) = 


'^p (27) - 


§( 0 ) = 


$( 1 ) = 


$(K) = 


§( 26 ) = 


0 . 6 

meter 

0 . 0 

meter 

0 . 5025 

meter 

0.6 

meter 

0.0 

meter 

0 . 5050 

meter 





90 degrees 
90 degrees 
0 degrees 


89.10 degrees 
89.40 degrees 
0.60 degrees 


88.20 - 1.8 (k-2> degrees 
88.80 - 1.2 (k-2) degrees 
1*2 +1.2 (k-2) degrees 


for K 


2 


. 25 . 


45.90 degrees 
60.60 degrees 
29.40 degrees 



§(27) 


45.00 degrees 

60.00 degrees 

50.00 degrees 
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— J 

-154.50 

degrees 

-75.52 

degrees 

-18 . 50 

degrees 

86 . 50 

degrees 

-25.76 

degrees 

-175.81 

degrees 

-154.50 

degrees 

-75.52 

degrees 

-18.50 

degrees 

-95.78 

degrees 

-25.76 

degrees 

4.19 

degrees 


(4.5-1) 


R&sxil ts with, MinirKum. Tim& Criterion 

In this case the objective function is defined by 

^•8 5) with weightages ^ and fi^ = 0. The redundancy in 

joint torque is resolved by the method outlined in Sec. 5.7. The 
eightages in torque resolution are taken as = 1 and = 0. 

cost matrices are same as in problem 1. The initial guess 
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are 

-2 


values of H(k) etc. are taken quite conservative Th 

0 =*ervative. These values 

taken as H (k>, = 2 sec AH - ? -1 

max - 2 sec . ah . = 

. . a. , ■ . min ^ s®e 

The io.nt velocity profiles are shoivn in Figs 4 fn t„ a 

‘ss. 4 . 28 to 4.31, From 

velocity profiles it is observed that ioint 5 of ar™ 1 reaches Us 

velocity li.it 2.1 rad./sec in the initial part of the path and 

joint 5 of arm 2 maintains this velocity upto the middle of the 

path. The joint 2 of arm 2 reaches its velocity limit 0.9 ^ad/sec 

in later half of the path. Thus the fastness n-r 

Tasxness of motion of the 

object is decided by these two joints The inin+ + 

joints, me joint torque profiles 

are shown in Figs. 4.32 to 4.33. From joint torque profiles it is 
Observed that joints 2 and 3 have larger torque variation because 
these joints mainly carry the load against gravity. The torque 
exerted by joint 3 of each arm is quite low as this joint 
primarily causes rotation of the object. One interesting thing is 
that at the end of first time interval no joint reaches its 
velocity limit or torque limit. Therefore the trajectory is 
completed without any joint reaching its torque limit. The object 
velocity profiles are shown in Figs. 4.36 and 4.37. The total 
electrical energy consumption was 10.83 Joules. The optimum values 
of H(k) are given in Table 4.9. The number of iterations required 
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with minimum time case for problem 2 





with minimum time case for problem 2 
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g. 4.36 Linear velocity of object with min,- 

j '~t- wicn nninimum 


time case for problem 2 



4.37 Angular velocity components of object 


with minimum time case for problem 2 


Table 4.9 Optimum time intervals anri • 

u.. c.e o: ::::::r‘ 


(interval number 
between kth and 
(k-l)th knot points) 


H (k) sec ^ 


At"(k-1) 
1/H*(k) ; 


16.11 

55.59 

57.06 

55.58 

55.04 

55.25 

55.49 
56.75 

57.50 

57.95 
58.65 
59.40 
40.16 

40.92 

41.69 

41.95 

40.70 

58.92 
56.06 
54.62 
54.64 
55.28 
51.94 

25.71 

24.55 

54.55 
25.67 


0.0620 
0.0187 
0.0270 
0.0282 
0.0285 
0.0284 
0.0282 
0.0272 
0.0268 
0.0264 
0.0259 
0.0254 
0.0249 
0 . 0244 
0.0240 
0.0258 
0.0246 
0.0257 
0.0277 
0.0289 
0.0289 
0.0500 
0.0515 
0.0589 
0.0407 
0.0185 
0.0590 


180 


to get the solution was 26. The "warm start” 

see ah = 20 see ^ and AH . = -20 

max 

convergence in 4 iterations. 


wi th 
see ^ 


H®<k) = 25 

yielded the 


4.3.2 Results with Minirnimi Energy Criterion 

In this case objective function is defined by 

(5.8-5) with weightages = 0 and /?2 = 1. For joint torque 

redundancy resolution the weightages taken are = 0 and ^2 = 1- 

The initial guess values of H<k). AH and AH . are taken same 

max min 

as minimum time case. The joint velocity profiles are shown in 
Fig. 4.58 of 4.42. Here joint 5 of arm 1 does not reach the 
velocity limit but joint 5 of arm 2 reaches the velocity limit 2.1 
rad/sec. and after few intervals maintains this velocity upto the 
middle of the path. But the joint 2 of arm 1 reaches its velocity 
limit 0.9 rad/sec near the middle of the path and maintains it in 
the later half of the path. In this case the velocity patterns 
are same as minimum time case but profiles are much smoother. The 
joint torque profiles are shown in Figs. 4.42 to 4.45. The torque 
patterns are also same as minimum time case but the torque values 
are in general lower. The object velocity profiles are shown in 
Figs. 4.46 and 4,47. Here also the velocity profiles are similar 
to minimum time case but the motions are smoothened. The optimum 
values of H*(k) are given in Table 4.10. The number of iterations 
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TabU 4.10 Optimum time intervals and reciprocal time intervals 
for minimum energy case of problem 2. 


k 

(interval number 

between kth and 

(k-l>th knot points) 

H (k) sec ^ 

At*(k-1) = 

1/H <k) sec 

1 

12.95 

0.0772 

2 

42.86 

0.0253 

? 

30.76 

0.0525 

4 

32.61 

0.0307 

5 

34.79 

0.0287 

6 

35.64 

0.0280 

7 

35.89 

0.0279 

8 

36.48 

0.0274 

9 

37.05 

0 . 0270 

10 

37.87 

0.0264 

11 

38.59 

0.0259 

12 

39.40 

0.0254 

13 

40.17 

0.0249 

14 

40.92 

0.0244 

15 

41.26 

0.0242 

16 

41.55 

0.0240 

17 

40.99 

0.0244 

18 

39.19 

0.0255 

19 

37.56 

0.0266 

20 

36.07 

0.0277 

21 

34.64 

0.0289 

22 

33.28 

0 . 0300 

23 

31.94 

0.0313 

24 

28.26 

0.0354 

25 

26.98 

0.0371 

26 

48.98 

0.0204 

27 

20.28 

0.0493 
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required is 124 and is much higher than the minimum time case. 
The energy required is 9.95 Joules. The total time of travel is 
0.81 sec. 


4» 4 Problem 3t Results and Discussions 

The problem 5 considers a parabolic path for the object. 
This path has ben selected to study a case where the velocities 
undergo reversal of sign. The object is initially at rest and is 
again brought to rest at the end of the trajectory. The steps of 
two solution stages are as detailed for problem 1, and differ only 
in terms of the input parameters. The important input parameters 
for this problem 5 are listed below, where the step numbers refer 
to those in section 4.2.1 and 4.2.2. Other input parameters are 
same as in problem 1. The base frames with respect to world 
coordinate frames are given by 




(4.4-1) 
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Input the object path and compute '^T^. For problem 5, the 
path is specified by 


0.6703 - 0.0031 (7.0 - 0.5l<)^ 

'^p <k> 0.14909 

^c 

0.4836 + .0031k 


meter 

meter 

meter 


for k = 0 , 


§(K> 


90 degrees 
90 degrees 
0 


for K = 0.1 27 


“120.74 

degrees 

-67.79 

degrees 

-8 . 37 

degrees 

-83.73 

degrees 

-81 . 90 

degrees 

j;_105.99 

degrees 

-141 . 37 

degrees 

-89.32 

degrees 

CO 

CD 

CD 

degrees 

-78.27 

degrees 

-39.62 

degrees 

-105.08 

degrees 


object 


1 27 


(4.4-2) 
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4. 4. i R&sxkl ts wi th Minimum. Tim» En&rg:^ Cri t&rion 

The results are presented for minimum time-energy case 
to show the nature of joint motions. The minimum time and minimum 
energy cases do not exhibit any additional features. In this case 
the objective function is defined by <3.8-3) with weightages 

= 0.8 and = 0.2. The redundancy in joint torque is resolved 
by the method outlined in Sec. 3. 7. The weightages in torque 

resolution are taken as = 0.5 and = 0-5. The cost matrices 

are same as in problem 1. The initial guess values of H(k>, AH 

max 

etc. are taken quite conservative. These values are taken as H®(k) 

= 2 sec , ^ sec , AH^.^ = -2 sec The parabolic path 

in x-z plane is shown in Fig. 4.48. The joint velocity profiles 
are shown in Figs. 4.49 to 4.52. The joint 1,2,3 and 5 of each 
arm change the joint velocity direction in the middle of the path. 
This is due to the motion of the object in parabolic path where 
motion along x direction is much more than z direction. At the 
middle of the path the x-compenent of object velocity changes the 
direction due to which the velocities of joints 1,2 and 3 change 
the signs. Joint 2 of arm 1 reaches the velocity limit 8.9 
rad/sec at initial part of the path and joint 2 of arm 2 reaches 
its limit at the end of the path. The joint torque profiles are 
shown in Figs. 4.53 to 4.56. No joint reaches its torque limit. 
The object velocity profile is shown in Fig. 4.57. The optimum 
values of H(k} are given in Table 4.11. The number of interations 





Joint velocity (rad/sec). ^ Joint velocity (rad/sec). 



tiaelseconds) 


Velocities of joints 1.2 and 3 for arm 1 
with minimum time-energy case for 
problem 5 



Fig. 4.50 Velocities of joints 4,5 and 6 for arm 1 
with minimum time-energy case for 


problem 3 


Joint velocity (rad/sec). ■ joint velocity (rad/sec). 



tinelseconds) 


1 Velocities of joints l.E and 3 for arm 2 
with minimum time-energy case for 
problem 3 



tiinelseconds) 

Fi9- 4.52 Velocities of joints 4,5 and 6 for arm 2 with 
minimum time-energy case for problem 3 





minimum time-energy case for problem 3 



tinie(seconds) 

Fig. 4.54 Torques of joints 4,5 and 6 for arm 1 
minimum time-energy case for problem 5 


with 


with 





Fig. 4.55 Torques of joints 1,2 and 5 for arm 2 
minimum time-energy case for problem 5 



tiine(seconds) 

Fig. 4.56 Torques of joints 4,5 and 6 for arm 


with 


2 with 


minimum time-energy case for problem 5 




196 


Table 4.11 Optimum values of reciprocal time intervals and time 
intervals in minimum time energy case for problem 3 


k 

(interval number 

between kth and 

(k-l)th knot points) 

H*(k) sec ^ 

At*<k-1) = 

1/H (k) sec. 

1 

6.40 

0.1562 

2 

16.76 

0.0596 

5 

17.04 

0.0586 

4 

18.61 

0.0537 

5 

25.91 

0.0386 

6 

27.47 

0.0364 

7 

31.10 

0.0321 

8 

30.71 

0.0326 

9 

36.01 

0.0278 

10 

44.68 

0.0224 

11 

32.30 

0.0309 

12 

14.24 

0.0702 

13 

13.74 

0.0728 

14 

46.79 

0.0214 

15 

72.60 

0.0138 

16 

70.07 

0.0138 

17 

62.98 

0.0159 

18 

60.86 . 

0.0164 

19 

49.76 

0.0201 

20 

42.18 

0.0237 

21 

32.91 

0.0304 

22 

28.93 

0 . 0346 
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Table 4.11 (Continued) 


25 

28.57 

0.0550 

24 

25.46 

0.0595 

25 

22.96 

0.0455 

26 

16.54 

0.0612 

27 

5.26 

0.1901 


required is 5Z. The energy required is 11.5 Joules. The total 
time of travel is 1.25 sec. 




CHAPTER 5 


CONCLUSIONS AND SCOPE FOR FUTURE WORK 

5. 1 Concl usi ons 

This thesis has presented two problems related to the 
coordinated motions of a dual-arm manipulator. The first problem 
dealt with the inverse kinematic solution when the two arms were 
involved in a peg-in-hole insertion type assembly operation and 
there was kinematic redundancy in the system. A specific example 
with two planar arms, each with three revolute joints, had been 
discussed. The kinematic redundancy had been utilized to perform 
the assembly operation with differently varying configurations 
depending upon the three local optimization schemes used. It was 
shown that as compared to the minimum velocity norm solution, the 
minimum acceleration norm solution is a better scheme since the 
joint velocities were then nearly constant with a marginal 
increase in the norm of joint velocities. On the other hand, the 
increased man i pu lab i 1 i ty criterion was shown to offer a good 
solution when the dual-arm system has to be taken away from the 
near singular configurations. It was also shown that a gradual 
increase in man i pu labi 1 i ty , as the assembly progresses, is a 
better scheme as it avoided the undue large movements of the 
joints . 

The second problem considered the dynamics of a dual-arm 
manipulator when the two arms formed a closed chain through a 
rigidly grasped object. This dual-arm system did not have 
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kinematic redundancy but had torque redundancy. The trajectory 


planning problem was considered for this 

system and a 

solut ion 

scheme as 

presented 

which combined 

the 

torque redundancy 

resol u t i on 

alongwi th 

minimization of 

t ime 

of travel 

and/or 

electrical 

energy over 

the trajectory. 

The 

result ing 

non! inear 


optimization problem was solved by using trapezoidal 
discretization scheme and then employing successive linear 
programming technique. Some numerical examples were presented 
with two PUMA 560 manipulators acting as the dual-arm system. The 
main points observed from the simulated results are listed below. 

The proposed scheme was particularly suitable in solving 
the minimum time problems. A proper warm start drastically 
reduced the number of iterations. But a careful selection of 
initial guess to reciprocal time intervals and the bounds on it 
was needed to warm start the program. Too small bounds on 
reciprocal time intervals did not satisfy the linear velocity 
constraints at the end of trajectory, and too large bounds 
resulted in violating the nonlinear torque constraints (though the 
linearized torque constraints in the linear programming part were 
satisfied). The minimum time objective was achieved by one or 
more joints reaching their respective torque and/or velocity 
limit(s>. It was noted that spacing between the knot points in 
the beginning should not be too large otherwise the joint torques 
3.re not maximally utilized. Problem Z of Chapter 4 showed that it 
is possible for the dual-arm system to execute minimum time motion 
without any joint reaching its torque limit (some joints obviously 
did reach their velocity limits). 
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Minimization of electrical energy over the trajectory 
was computationally not efficient. In this case the warm start 
also did not help and the convergence was very slow in the later 
stages of iterations, probably because this problem is highly 
nonlinear. The time of travel was not unduly large <as would have 
been the case if only velocity and acceleration terms were 
considered) due to presence of gravity induced torques. As the 
notions were mu ch slower, no joint limits were reached. 

Minimum time and energy criterion yielded intermediate 
results. Inclusion of energy term with a low weightage helped in 
smoothening the motions, though increasing the computation time. 


5. 2 Scope f or Future Work 

This thesis had dealt with two isolated problems: first 
one with only kinematic redundancy and the second one with only 
torque redundancy. The next step would be to have a dual-arm 
system which has both kinematic and torque redundancies. The 
trajectory planning problem can be further investigated with 
inclusion of jerk constraints as this would be expected to produce 
much smoother motion. Another important aspect would be to 
include the internal forces (i.e. the forces on the object by the 
end effectors which do not contribute to the motion of the object 
but only create internal stresses in the object) in the 
optimization scheme. This could be, attempted by putting the 
minimization of internal forces in the objective function, or by 
having the limits on internal forces as inequality constraints. 



It was pointed out that 
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successive linear programming 
technique yielded a very slow convergence for minimum energy 
problem, which was highly nonlinear. It is suggested that 
successive quadratic programming technique may be tried to achieve 
a faster convergence. 
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APPENDIX A 


EULER ANGLES AND THE TRANSFORMATIONS 

A set of three generalized coordinates can completely 
describe the orientation of a rotating rigid body with respect to 
a cartesian reference frame. One common choice is that of Euler 
angles denoted by , <t>^ and (shown in Fig. A.l). The 

representation used here corresponds to the following sequence of 
rotat i ons . 

Initially let the object frame and the reference frames 
coincide CFu et al., 1987]. 

1. A rotation of angle about the Z axis of the reference 
f rame . 

2. A rotation of angle about the current X axis of the object 
f rame . 

5. A rotation of 4>^ angle about the current Z axis of the object 
f rame . 

The resultant eulerian rotation matrix is given by 



- COS^J 

-sin^j 

0 1 


1 

0 

0 1 

R = 

s i r\4>. 

COS<^j^ 

0 


0 

cos^2 

-s in <^2 


X 

- 0 

0 

1 • 


. ® 

sin<^2 

cos^2 


j. cos^^ -sin<^^ 

I sin^^ cosip^ 


0 


0 


1 
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cos4>^ cos4>^ 


•cos^jS iruj^j 


-si r\4>-^ cos^2® i ^’P-^ “S i cos^-cos0. 


sin^.sin*^. 


s i n<^j cos^j 
+ cos^j cos^2® i'^'^5 


-s i n<^jS 

+ C05^jC0S<^2'^0S<^^ 


■cos^jSin^2 


s i n4>2^ i r\4>-^ 


s'\r\<p^cos4>^ 


cob 4>. 


(A.l) 

The inverse solution implies that a rotation matrix is 
given and the corresponding Euler angles are to be determined. If 
the rotation matrix is given in terms of direction cosines as 


n s a 

XXX 


n s a 

y y y 


n s a 
z z z 


(A.2> 


then the Euler angles are determined by the following relations 


= tan 


- tan 


-cos<^j s^ - sin<^2 Sy 
cos<^j n^ + sin^j n^ 


= tan 


^ r sin<^ja^ - cos<^ja^ 


<A.3) 
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The angular velocity of an obiect ran 

7 ooject can be expressed in 

terms of its components alonq the avoe n* _ i. ■ 

^ of cartesian reference 

frame, i.e. as Q = i +a>^ j ^ . Alternately. the angular 

velocity could be written in terms of time rate of change of Euler 

angles, i.e. in terms of and ^ The relation between w 

X ’ 

and <^ 2 - <5^3 established in the following way. The 

rotation matrix R is an orthogonal matrix i.e. 

R R^ = I 

where I is a 3x3 identity matrix. 

Differentiation of (A. 4) yields 


(A. 4) 


^^(R) r"’’ + R 
dt 


dt 


(r"'') = 0 


or. 


dt 


{-S! 


(A.5> 


As R is rotation matrix its columns are orthogonal to 
each other and each is an unit vector. Hence as R changes with 
time its column vectors change orientation with rotation of the 
object. When the object frame rotates with angular velocity Q = 
['^x ' ^y ' each column vector of R also rotates with that 
velocity. The time rate of change of R can be expressed as 


dR 

dt 


dn ^ ^ 

Ht dt dt 


= [axn OxsOxa] (A.6> 

The cross product of O can be replaced by a skew symmetric matrix 



as 





-a> 

z 

CO 


= 

N 

3 

0 

“CO 



L-a> 

y 

X 

3 

0 


as 


(A. 7 ) 


Using 


( A . 7 ) in 

dt 


(A. 6) 

[n s a] 




<A. 8 ) 


Now using (A. 8) in <A.5) 


Using <A.4) in (A. 9) 

R <A.i 0 ) 


Using (A.l> and <A.7) in (A.10> the time rate of change of 


angles vector § = 



’ ^5 J angular velocity 

is determined as follows 


Euler 

vector 


f5 


0 

0 


COS0J 

s i n^j 


s i s i n <^2 
-COS0J sin^2 

cos<^2 


f 


<A.ll) 


1 


0 



appendix b 


MANIPULATOR JACOBIAN 


The end effector motion of an n degrees-of-f reedom 
manipulator is described by two vectors, a translational velocity 
vector V and an angular velocity vector O, where v is the 
translational velocity of the origin of the hand coordinate frame, 
while a is the angular velocity of the hand frame. A six element 
vector s representing these task space velocity vectors can be 
written as [Whitney, 19723 

i - [ ^ ]. (B.l) 

The joint angle vector is denoted by q and joint velocity vector 
by q, where each of these vectors is n dimensional vector. The 
transformation matrix of jth coordinate frame (attached to jth 
link) with respect to world coordinate is obtained from (2.5-2) 
and (2.3-3) and can be written in the following from : 


1 

1 

II 

1 

w 
s . 

) 

w 
a . 

J 

W 1 

Pj 

[ 0 

1 


1 

0 

0 

1 — 1 


Let V and O. respectively denote the end effector translational 
and rotational velocity vectors due to the jth rotational joint 
velocity q. alone (i.e.. all other joints, except the jth joint . 
are locked). The direction of q. is along the Z-axis of (j-l> th 
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coordinate frame, i.e. along the unit vector a 

i-1 ■ 

angular velocity of the end effector frame due to 
joint alone is written as 


Therefore the 
jth rotational 




(B.>> 


The position vector of the end effector from the origin of (j-2) 
fw w 

th frame is I P^, " *^j-lj translational velocity of the 

end effector due to jth rotational joint alone is 


V . 

J 





<B.4) 


The net end effector velocity vector is obtained by sunroing (B.3) 


and (B.4) over all the joints to get 

n 


V = 


2 h 


n 



j=i 


<B.5> 


(B.5> to <B.5) are now combined and written in matrix notations as 


♦ T ♦ 

s = J q 


<B.6) 


where J is a 6xn matrix, called Jacobian matrix, whose jth element 
is given by 


J . 


] 





for j = 1 . . - , n . 


(B.7> 



APPENDIX C 


KINEMATIC RELATIONS FOR PLANAR DUAL-ARM MANIPULATOR 


This 3.pp6nciix lists ths ki neniat i c rriaitr icss usGd for the 

planar dual-arm manipulator (each arm with three links and three 

rotary joints) which was used as an example in Chapter 2. For the 

sake of continuity, Fig. 2.3 is reproduced here as Fig. C.l which 

shows all the link frames denotes the arm number, 

i.e.l= 1,2 and i denotes the link number, i.e. i = 0,1, 2, 5). The 

world frame X , Y is taken to be coincident with the base frame 
w w 

of first arm X,^, The separation between the bases of two 

10 10 

arms is denoted by . Therefore the transformation matrices of 

w 

the two base frames < in (2.5-23)> are 


w. 


10 


1 0 
0 1 
0 0 
0 0 


0 0 
0 0 
1 0 
0 1 


and 


w. 


20 


1 


0 

1 


0 b 


0 0 
0 0 10 
0 0 0 1 


<C.1> 


In Fig. C.l, the link lengths are denoted by a^. and the 
angles by . <1 = 1,2; i = 1.2.3). For jth link of 1th arm. 
matrix of (2.3-1) becomes 


joint 
the A 


j-1 


A 


1 j 


cosy' 
s i ny' 
0 
0 


-s iny 
cosy' 
0 
0 


0 e 

0 f 

1 0 

0 1 


<C.2) 




where , 


•220 


. = I 

i=l 

i i 

e = ^ aj. cos 

i=l 1<=1 

f ^ a,. Sin 

i=l k=l 

The end effector frame of arm 1. i.e. is described in 

the world frame by the transformation which is partitioned in 

IaI 

(2.5-7) into rotation matrix and the position vector Pj^- 

With the help of (C.2>, (2.5-2), (C.l) and (2.5-5) the description 


1 ^ in the world 

frame is 


COS(qii+qi2+qi3> 

-Sin<qjj+qj2+qj,) 

0 


sin(qii+qi2+qi j) 

COS(qjj+qj2+qj,) 

0 

(C.5) 

0 

0 

1 


__ 


— 
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ail ‘^ll'''^12 COS(qii+qi2) + ^ij COS(qii+qi2+qi3> 


ail "^ll‘^^12 sin(qii+qi2> + ^i^ sin(qii+qi2+qi5> 


(C.4> 


L ® J 

Likewise, for arm 2 the end effector frame is represented 

in the world frame by 
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-Sintq^j+q^^+q^^, 

0 

= ‘"‘'='2l'"‘’22+'’25> 

COS(q2;.q^2+‘’2,> 

0 

0 

0 

1 


w 


23 


^21 °‘2l'‘'^22 ^ ^23 '^°®^‘^2l'*''^22'*''^23^'*‘*^s 


a^i sin q21^^22 sin<q 2 j+q 22 > + sin(q 2 j+q 22 +q 25 ’ 

0 


(C.6> 

The solution (2.5-25) requires the evaluation of J matrix, which 

r 

in turn requires J and J , as mentioned by (2.4-26). The 

er rel 

expression for Jgj, is given in (2.4-27). Evaluation of from 

(2.4-22) to (2.4-24) also requires and besides (C.3) to 

(C.6). These Jacobian matrices for the two arms of Fig. C.l can 
be obtained as 



1.2 


(C-7) 



APPENDIX D 


INERTIAL COEFFICIENTS 


The objective of this appendix is to derive the inertial 
coefficients needed in Sec. 3.9.3. These derivations follow the 
work of Tourassis and Neuman C19833. A manipulator with N 
articulated joints is considered. The generalized coordinates of 
N degrees of freedom mechanism are The generalized 
coordinate q. is defined as the rotational angle around axis 
of link i -1 . 

The kinetic energy of the manipulator, denoted by KE , 
equals the sum of the kinetic energies of the point masses which 
constitute the elements of the manipulator and is given by 


KE = 4- y rn <r .r ) 
2 Z a a a 


(D.l> 


where. r = r <q, q„,) is the position vector of point itass 

a a 1 N 

m in the base frame of the manipulator. Thus, 
a 


N ^ 

V a ♦ 
r = > -X q . 


< 0 . 2 > 


Substitution of (D.2) in (D.l) yields 


N N 


KE = 




(D.3> 
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where, the coefficients d. . are given by 



If the point mass m belongs to link ij , then r = 

a ^ 


r"- (q, . . . .q ) 

a 1 


i , j ) 


_ y. dr . ^ dr 

2u ■ ( ^"] 


<D.4> 


Since is the position vector from the origin of the base frame 
to the point mass m .one can write (see Fig. D.l) 

oL 


= r . + r . . + p . 

1 1 3 ^3a 


r . + p . 

1 ^la 


(D.5) 


It is implied in (D.5) that j > i is being considered 
where , 

r. is the position vector from the origin of the base frame 
to the origin of frame (i-1), 

r. . is the position vector from the origin of frame (i-1) to 
I 3 

the origin of frame (j-1), 

p. is the position vector from the origin of frame (i-1) to 
^ la 

the point mass m in link p ^ it and 

a 

p. is the position vector from the origin of frame (J-l) to 
3 a 

the point mass m in link ^ j . 

a 

The partial derivatives of r in (D.5> with respect to the 

a 

generalized coordinates q. and q. are now written as (j ^ i) 

1 ] 




Fig. D.l Position vectors 


in a kinematic chain 


with rotary joints 
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^i-1 ^ Pi 


(D.fe> 


dp . 

3a. 

-3 - z . , X p . 

^q: 3-1 *^3a 


(D.7) 


Substituting <D.6> and (D.7) into (D.4), the following explicit 
formula for the inertial coefficients is obtained 


d. . = y ) m ■ 1 X P ■ > ■ (z . , X P . ) ; 3 > 


i (D.8> 


The normal, sliding, approach and translation vectors of the 

transformation matrix of (j-1) th frame with respect to (i-1) th 

.. , , . 

frame all denoted by Pj-l 

respect i ve 1 y . 


z . , X p . 

1-1 ja 


s . , p - 
3-1,2 ^3az 


^3-1,2 Pjay] ' 


fi-l 

i "i-i 


i“l 

p . - a . 

jaz ] 


,z Piax)'[‘’'"i-l.z ” 


i-1 

jay' ^ 3 -l.z ^ia-x, 


(D.9> 


and z. , X p. = “Pi,,, Piav ® 

3-1 *^3 a I 3 ay 3 ax 


(D.10) 


Using (D.9> the following simplification of the expression under 


summation in (D.8) is obtained. 


h-l X Pia] • h-1 '■jj ‘ h-' 
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(D.Il) 


The first term on the right hand side of (D.ll) defined in base 
frame can be expressed with respect to (i-l) th frame to get 




n . , 

) -1 , X 


^ j -1 . X 


i-l 


P; 


j-1 .y 


i-l 


j-1 .y 


i-i 1 

"i-l.y h-l,xj ’“jay 

i-l i-l 1 

j-1 iX *^i-l ,xj ^jax‘ 


(D.IZ) 


Similarly the second term on the right hand side of (D.ll) defined 
in base frame can be expressed with respect to (j-Dth frame to 
get 


['-'Zi-i x’-' p.J . X '-'pjJ 


‘’*^)-I,z Pjay Pjaz ' ‘’’"j-l.z hax haz 


1 - 


'a. , f, 
j - 1 . z L 


2 ^21 
•^jax jayJ 


(D.15) 


From (D.9), (D.10), (D.ll), (D.12) and (D.15> yields 


h-l " Pia) • h-1 ’’ '■ij ' 
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i -1 i -1 ^ 

^jay 

i -1 i -1 'I 

Pj-l.xJ Pjax 


fi-l i -1 

I "J-I.x Pj - l.y 


1-1 

s • , P . p . 

] - 1 ,z ]ay jaz 


i-1 i-1 

n . , p . p . + a 

j- 1 .z 3 ax ^jaz - 1 ,z 



<D. 14) 


Before further simplification of j the expressions for 
pseudo- inert ia matrices are to be defined. For any link of an arm 
the mass, moments of inertia, products of inertia and position 
vector of the centre of mass of kth link from the axis of rotation 
expressed through pseudo inertia matrix as follows: 




-Hi 




+^I 


fjxx tuyy nzz 




fuxy 




IJXZ 


m X 


^xy 


jJXZ 


-^i 


)UXX ^yy jUZZ /Jj 

2 luyz 


m X 
4* F* 


m y 




+^I 


.F<- 


Ijxx fjyy yLzz 


/Liyz 


m y 


m z 


m z 

hi 


m 


(D.15) 


where . 

F' t 3 ^j.@ the moments of inertia of /j th link 

Vxx' Vyy’ Mzz 

in 3 L 1 th frame , 





t^r M T Hr 

Hxy' MVZ’ iuxz products of inertia, 

is the mass of ^ th link. 


position vector of the centre 


of mass, 


The pseudo-inertia matrix is transformed from h th 

coordinate frame to j— 1 th frame by the following relation. 


j H 


T 

H H 




(D.16> 


To determine d.. it is required to transform the moments of 
inertia of all the links beyond j-1 and add to get the effective 
inertia of those links at the joint j which is given by 


-J. 


'-h . 

H 


(D.17) 


Now the moments of inertia, products of inertia and mass 
corresponding to are determined in terms of its matrix elements 
by the following relations 


IN 

<4, 4) =5^1^ 
k^j 

= I V <1 .4)/m^ . 


y^ = <2.4)/m^ . 


= 1^(2. 2) + I^<5.3) . 

XX 

1^ = 1^1, 1> + I^<5.3) . 

yy 

I ^ = I ^ (1 .1 ) + I ^ (2.2) , 

ZZ 


<3,4)/m^ . 


I ^ = I J <1 .3) 

xz 


= 1^(1. 2) . 

xy 

= 1 ^( 3 , 2 > . 


(D. 18) 


The quantity (il.jl) in <D.18) is the element of in il th 


row and jl th column. 
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Now using (D.14) and (D.18> in (D.8) theexpress i on for 

d . . becomes 
1 } 


d. . 
1 J 


j -1 . z 


J 

zz 


i-1 


s . 


j -1 . z yz 


i-1 ^ j 

J -1 , z xz ) N 


/i-1 

( n 


i-1 


j -1 , X 


P ; 


i-1 


j-1 .y 


n . 


i-1 


j-1 .y 


Pj-I.x’ 


y - 


i-1 


s . 
J 


i-1 


i-1 


1.x 


j-1 .y 


s . 


i-1 


j-l.y 


j -1 . X^ 




<D. 19) 




APPENDIX E 


INVERSE POSITION KINEMATICS OF PUMA 560 


This appendix gives the inverse kinematics solution of 
PUMA 560 when the transformation matrix of end effector frame with 
respect to base frame (3.3-5) is given. The details of the method 
CFu et al., 1987] are not discussed here, and only the expressions 
for joint angles are given. For a particular end effector 
position and orientation the arm, elbow and wrist nay have 
different onf i gurat i ons which are described by three indicators 
ARM, ELBOW and WRIST. The values of these indicators for 
different configurations are listed below. 


ARM 


+1 RIGHT arm 
-1 LEFT arm 

L. 


(E.l) 


ELBOW 


+1 ABOVE arm 
-1 BELOW arm 


(E.Z) 


WRIST 


+1 WRIST DOWN 
-1 WRIST UP 


<E.3) 


Using the conventions used in (2.5-2) the given trsnstormation 

matrix ®T. (for which the loinl angles are to be calculated) is 
6 

written in terms of its elements as follows; 
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6x 

®s 

fax 

0 

^6x 

0 1 
P6x I 


6y 

®s 

6y 

0 

6y 

0 

^6y 


6z 

0c 

0 

0 


6z 

^6z 

^62 

. (E.4) 

0 

0 

0 

1 



The expression for first joint angle is given by 


= tan 


ARM /0 2 ^ 0 2 Z 

'’6y ^ P6y * P6y ' ''z 


ARM /\l + W - 4 

•^ex y '^6x ^6y 2 


P6x 


+ P^.. d 


6y °2 j 


<E.5> 


The second joint angle is calculated from 


rsin<q )' 

q„ = tan ^ - n < q, < n 

2 cos<q2) 2 


(E.6) 


where, 

sin q^ = sin(alpl> cos(betl) + (ARM. ELBOW) cos<alpl> sin(betl>, 

(E.7> 

cosq^ = cos<alpl) cos<betl) - (ARM. ELBOW) sin(alpl) sin(betl), 

(E.8) 


sin(alpl) = 


0 j /~0 2 7 0 2 7 0 2 72 

Pfez '^6x ^6y ^6z ^2 


(E.9) 


cos (alpl) = 


/T~l ,02 

- arm -/ + P6y 


/ 
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0 2 0 2 —rr 

P6x + Pev Pa: 


<E.10> 


cos < betl ) = 


r* 

0^2 , 02,02 2 2 2 2I . 

^6x P6y ^6z ~ ~ ^^^4 ^ ^5^ / 

'■ 


o, y 0 2 
=^^ 2 ^ P6X 


4. , 0„2 .2 

* '’6y * ^62 - ''2 


where , 


The fourth joint angle is given by 


(E.ll) 


s i n ( bet 1 ) 


1-cos <betl> 


(E.12> 


The third joint angle is given by 


= tan 


_2 "^3 

cos q. 


-TT < q^ < TT 


(E.15> 


sin q^ = sin(phi2> cos(bet2) - cos(phi2) sin<bet2) 


(E.14> 


cos q^ = cos(phi2) cos<bet2> - sin(phi2) sin(bet2) 

2 2 2 , [02,02^02 

cos(phi2) = + <d^ + a^) - [ p^^ + P^y + 


(E. 15) 


<E.16> 


sin (phi 2) = ARM. ELBOW y 1-cos (phi 2) 


(E.17) 


sin (bet2) = 


d / / d^ + 

4 4 > 


(E. 18) 


cos (bet2) = 


a^ll / / d^ + 


(E.19) 


= tan 


1 P 4 

cos q- 


(E.20) 


where , 
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COS q 


+ sin sin (q^, + q^> ®n^^ <E.>0> 

g = [-sin qj cos q^ - cos q^ cos (q^ + q^)+ sin qj ®s 
+ [cos cos q^ - sin q^ cos<q 2 + q^> sin qj°s^y 
+ sin q^ sin (q^ + q^) ®s^^ <E.31) 




APPENDIX F 


KINEMATIC AND DYNAMIC PARAMETERS OF PUMA 560 MANIPULATOR 


In Chapter 4 the trajectory planning problem is solved 
for a dual-arm manipulator consisting of two PUMA 560 arms. For 
the computation of different quantities needed in trajectory 
planning, various geometric, kinematic and dyanamic parameters of 
PUMA 560 are required. These parameters are presented sequentially 
in this appendix. 

The geometric parameters of PUMA 560 are given in Table 
F.l [Fu et al . , 1987]. The geometric parameters are link lengths 
a^ offset dimensions a. and d. (see < 2 . 3-1 ) ) , range of joint 
angles etc. 

Table F.l Geometric parameters of PUMA 560 


Joint i 

a . 

1 

(degrees ) 

a . (mm) 

1 

d . (mm) 

1 

Range of joint angle 

q. (degrees) 

1 

-90 

0 

0 

-160 to + 160 

2 

0 

431.8 

149.09 

-225 to 45 

3 

90 

-20.32 

0 

-45 to 225 

4 

-90 

0 

433.07 

-110 to 170 

5 

90 

0 

0 

-100 to 100 

6 

0 

0 

56.25 

-266 to 266 


Table F.2 lists the link masses, radii of gyration. location of 




centres of mass in the respective link frame (see Fig. D.l>. 


Table F.2 Inertia parameters of PUMA 560 CTarn and Bejczy. 1988] 


Link 

Mass 

Distance of 

centre 

Square 

of radius 




of 

mass 


of 

gyrat i on 


i 

m - 
1 

X . 

1 

^i 

2 . 

k^ 

1 xz 

k^ 

lyy 

k^ 

1 zz 


(kg) 

(m) 

(m) 

(m) 

(m^) 

(m^) 

(m^> 

1 

12.96 

0.0 

0.5088 

0.0589 

0.1816 

0.0152 

0.1811 

B 

22.57 

-0 . 5289 

0 .0050 

0.2058 

0.0596 

0.1950 

0.1514 

■ 

6.97 

0.0156 

0.0092 

0.1522 

0.0785 

0.0786 

0.0021 

H 

1.18 

0.0 

0.0865 

-0.0029 

0.0119 

0.0029 

0.0118 

■ 

0.62 

0.0 

•0.0102 

0.0015 

0 . 0009 

0 . 0009 

0 . 0009 

H 

0.16 

0.0 

0.0 

0.0029 

0.0008 

0 . 0008 

0 . 0004 


The principal moments of inertia are then calculated as 


I . 

1 XX 

I . 
lyy 

I . 


1 zz 


m. 

1 1 XX 

m. 

I lyy 

m. kf 
1 1 zz 


(F.l> 


The products of inertia 
inertias from (F.l) are 
by (D.15) of Appendix D 
limit depending on the 
velocity limits come 
constraints (5.10-6) of 


terms are assumed to be zero 

• 

These 

used to calculate pseudo- inert ia 

matr i ces 

Each joint of PUMA 

560 has a 

velocity 

servometer at the 

joint. 

The 

joint 

as constraints are 

requ i red 

i n 

the 

trajectory planning 

problem. 

The 

joint 


velocity limits are given 


in Table F . 3 . 








£37 





The gear ratios used in (3.7-5) are given in Table F.5. 

Table F.5 Gear ratios (c^> of PUMA 568 CNagy.1988] 
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In the minimum electrical energy considerations. the 
motor constant and the power supply resistance c"^ are required 
in (3.7-5). These constants are listed in Table F.6. 


Table F.6 Motor constants and power supply resistances of 
Servomotors CNagy, 1988] 


Jo i nt 

1 

2 

3 

4 

5 

6 

r 

Ci 

(Ohm) 

1 .6 

1.6 

1 .6 

CO 

3.8 

3.8 

m 
c . 

1 

(Kg . cm/A) 

2.58 

2.58 

2.58 

0.973 

0.973 

0.973 








